ViSP  2.8.0
vpKalmanFilter.cpp
1 /****************************************************************************
2  *
3  * $Id: vpKalmanFilter.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Kalman filtering.
35  *
36  * Authors:
37  * Eric Marchand
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 
48 #include <visp/vpKalmanFilter.h>
49 
50 #include <math.h>
51 #include <stdlib.h>
52 
63 void
64 vpKalmanFilter::init(unsigned int size_state, unsigned int size_measure,
65  unsigned int nsignal)
66 {
67  this->size_state = size_state;
68  this->size_measure = size_measure ;
69  this->nsignal = nsignal ;
70  F.resize(size_state*nsignal, size_state*nsignal) ;
71  H.resize(size_measure*nsignal, size_state*nsignal) ;
72 
73  R.resize(size_measure*nsignal, size_measure*nsignal) ;
74  Q.resize(size_state*nsignal, size_state*nsignal) ;
75 
76  Xest.resize(size_state*nsignal) ; Xest = 0;
77  Xpre.resize(size_state*nsignal) ; Xpre = 0 ;
78 
79  Pest.resize(size_state*nsignal, size_state*nsignal) ; Pest = 0 ;
80 
81  I.resize(size_state*nsignal, size_state*nsignal) ;
82  // init_done = false ;
83  iter = 0 ;
84  dt = -1 ;
85 }
86 
94 {
95  verbose(false);
96  // init_done = false ;
97  this->size_state = 0;
98  this->size_measure = 0 ;
99  this->nsignal = 0 ;
100  iter = 0 ;
101  dt = -1 ;
102 }
103 
111 vpKalmanFilter::vpKalmanFilter(unsigned int nsignal)
112 {
113  verbose(false);
114  // init_done = false;
115  this->size_state = 0;
116  this->size_measure = 0 ;
117  this->nsignal = nsignal;
118  iter = 0 ;
119  dt = -1 ;
120 }
121 
134 vpKalmanFilter::vpKalmanFilter(unsigned int size_state, unsigned int size_measure, unsigned int nsignal)
135 {
136  verbose(false);
137  init( size_state, size_measure, nsignal) ;
138 }
139 
155 void
157 {
158  if (Xest.getRows() != size_state*nsignal) {
159  std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()
160  <<" " << size_state*nsignal<< std::endl ;
161  std::cout << " Error : Filter non initialized " << std::endl;
162  exit(1) ;
163  }
164 
165 // if (!init_done) {
166 // std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<" " << size_state<< std::endl ;
167 // std::cout << " Error : Filter non initialized " << std::endl;
168 // exit(1) ;
169 // return;
170 // }
171 
172  if (verbose_mode) {
173  std::cout << "F = " << std::endl << F << std::endl ;
174  std::cout << "Xest = "<< std::endl << Xest << std::endl ;
175  }
176  // Prediction
177  // Bar-Shalom 5.2.3.2
178  Xpre = F*Xest ;
179  if (verbose_mode) {
180  std::cout << "Xpre = "<< std::endl << Xpre << std::endl ;
181  std::cout << "Q = "<< std::endl << Q << std::endl ;
182  std::cout << "Pest " << std::endl << Pest << std::endl ;
183  }
184  // Bar-Shalom 5.2.3.5
185  Ppre = F*Pest*F.t() + Q ;
186 
187  // Matrice de covariance de l'erreur de prediction
188  if (verbose_mode)
189  std::cout << "Ppre " << std::endl << Ppre << std::endl ;
190 }
191 
223 void
225 {
226  if (verbose_mode)
227  std::cout << "z " << std::endl << z << std::endl ;
228  // Bar-Shalom 5.2.3.11
229  vpMatrix S = H*Ppre*H.t() + R ;
230  if (verbose_mode)
231  std::cout << "S " << std::endl << S << std::endl ;
232 
233  W = (Ppre * H.t())* (S).inverseByLU() ;
234  if (verbose_mode)
235  std::cout << "W " << std::endl << W << std::endl ;
236  // Bar-Shalom 5.2.3.15
237  Pest = Ppre - W*S*W.t() ;
238  if (verbose_mode)
239  std::cout << "Pest " << std::endl << Pest << std::endl ;
240 
241  if (0) {
242  // Bar-Shalom 5.2.3.16
243  // numeriquement plus stable que 5.2.3.15
244  vpMatrix Pestinv ;
245  Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU()*H ;
246  Pest = Pestinv.inverseByLU() ;
247  }
248  // Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
249  Xest = Xpre + (W*(z - (H*Xpre))) ;
250  if (verbose_mode)
251  std::cout << "Xest " << std::endl << Xest << std::endl ;
252 
253  iter++ ;
254 }
255 
256 
257 #if 0
258 
259 
309 void
310 vpKalmanFilter::initFilterCteAcceleration(double dt,
311  vpColVector &Z0,
312  vpColVector &Z1,
313  vpColVector &Z2,
314  vpColVector &sigma_noise,
315  vpColVector &sigma_state )
316 {
317  this->dt = dt ;
318 
319  double dt2 = dt*dt ;
320  double dt3 = dt2*dt ;
321  double dt4 = dt3*dt ;
322  double dt5 = dt4*dt ;
323 
324  //init_done = true ;
325 
326  Pest =0 ;
327  // initialise les matrices decrivant les modeles
328  for (int i=0; i < size_measure ; i++ )
329  {
330  // modele sur l'etat
331 
332  // | 1 dt dt2/2 |
333  // F = | 0 1 dt |
334  // | 0 0 1 |
335 
336  F[3*i][3*i] = 1 ;
337  F[3*i][3*i+1] = dt ;
338  F[3*i][3*i+2] = dt*dt/2 ;
339  F[3*i+1][3*i+1] = 1 ;
340  F[3*i+1][3*i+2] = dt ;
341  F[3*i+2][3*i+2] = 1 ;
342 
343 
344  // modele sur la mesure
345  H[i][3*i] = 1 ;
346  H[i][3*i+1] = 0 ;
347  H[i][3*i+2] = 0 ;
348 
349  double sR = sigma_noise[i] ;
350  double sQ = sigma_state[i] ;
351 
352  // bruit de mesure
353  R[i][i] = (sR) ;
354 
355  // bruit d'etat 6.2.3.9
356  Q[3*i ][3*i ] = sQ * dt5/20;
357  Q[3*i ][3*i+1] = sQ * dt4/8;
358  Q[3*i ][3*i+2] = sQ * dt3/6 ;
359 
360  Q[3*i+1][3*i ] = sQ * dt4/8 ;
361  Q[3*i+1][3*i+1] = sQ * dt3/3 ;
362  Q[3*i+1][3*i+2] = sQ * dt2/2 ;
363 
364  Q[3*i+2][3*i ] = sQ * dt3/6 ;
365  Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
366  Q[3*i+2][3*i+2] = sQ * dt ;
367 
368 
369  // Initialisation pour la matrice de covariance sur l'etat
370 
371  Pest[3*i ][3*i ] = sR ;
372  Pest[3*i ][3*i+1] = 1.5/dt*sR ;
373  Pest[3*i ][3*i+2] = sR/(dt2) ;
374 
375  Pest[3*i+1][3*i ] = 1.5/dt*sR ;
376  Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
377  Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
378 
379  Pest[3*i+2][3*i ] = sR/(dt2) ;
380  Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
381  Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;
382 
383 
384  // Initialisation pour l'etat
385 
386  Xest[3*i] = Z2[i] ;
387  Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
388  Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;
389 
390  }
391 }
392 
393 void
394 vpKalmanFilter::initFilterSinger(double dt,
395  double a,
396  vpColVector &Z0,
397  vpColVector &Z1,
398  vpColVector &sigma_noise,
399  vpColVector &sigma_state )
400 {
401  this->dt = dt ;
402 
403  double dt2 = dt*dt ;
404  double dt3 = dt2*dt ;
405 
406  double a2 = a*a ;
407  double a3 = a2*a ;
408  double a4 = a3*a ;
409 
410  //init_done = true ;
411 
412  Pest =0 ;
413  // initialise les matrices decrivant les modeles
414  for (int i=0; i < size_measure ; i++ )
415  {
416  // modele sur l'etat
417 
418  // | 1 dt dt2/2 |
419  // F = | 0 1 dt |
420  // | 0 0 1 |
421 
422  F[3*i][3*i] = 1 ;
423  F[3*i][3*i+1] = dt ;
424  F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
425  F[3*i+1][3*i+1] = 1 ;
426  F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
427  F[3*i+2][3*i+2] = exp(-a*dt) ;
428 
429 
430  // modele sur la mesure
431  H[i][3*i] = 1 ;
432  H[i][3*i+1] = 0 ;
433  H[i][3*i+2] = 0 ;
434 
435  double sR = sigma_noise[i] ;
436  double sQ = sigma_state[i] ;
437 
438  R[i][i] = (sR) ; // bruit de mesure 1.5mm
439 
440  Q[3*i ][3*i ] = sQ/a4*(1-exp(-2*a*dt)+2*a*dt+2*a3/3*dt3-2*a2*dt2-4*a*dt*exp(-a*dt) ) ;
441  Q[3*i ][3*i+1] = sQ/a3*(1+exp(-2*a*dt)-2*exp(-a*dt)+2*a*dt*exp(-a*dt)-2*a*dt+a2*dt2 ) ;
442  Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;
443 
444  Q[3*i+1][3*i ] = Q[3*i ][3*i+1] ;
445  Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt ) ;
446  Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;
447 
448  Q[3*i+2][3*i ] = Q[3*i ][3*i+2] ;
449  Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
450  Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;
451 
452 
453  // Initialisation pour la matrice de covariance sur l'etat
454  Pest[3*i ][3*i ] = sR ;
455  Pest[3*i ][3*i+1] = 1/dt*sR ;
456  Pest[3*i ][3*i+2] = 0 ;
457 
458  Pest[3*i+1][3*i ] = 1/dt*sR ;
459  Pest[3*i+1][3*i+1] = 2*sR/dt2 + sQ/(a4*dt2)*(2-a2*dt2+2*a3*dt3/3.0 -2*exp(-a*dt)-2*a*dt*exp(-a*dt));
460  Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1) ;
461 
462  Pest[3*i+2][3*i ] = 0 ;
463  Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2] ;
464  Pest[3*i+2][3*i+2] = 0 ;
465 
466 
467  // Initialisation pour l'etat
468 
469  Xest[3*i] = Z1[i] ;
470  Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
471  Xest[3*i+2] = 0 ;
472 
473  }
474 }
475 
476 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
void filtering(vpColVector &z)
long iter
Filter step or iteration. When set to zero, initialize the filter.
unsigned int size_state
Size of the state vector .
vpMatrix R
Measurement noise covariance matrix .
unsigned int size_measure
Size of the measure vector .
vpColVector Xest
vpMatrix I
Identity matrix .
unsigned int nsignal
Number of signal to filter.
void verbose(bool on)
void init(unsigned int size_state, unsigned int size_measure, unsigned int nsignal)
vpMatrix t() const
Definition: vpMatrix.cpp:1176
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
vpColVector Xpre
vpMatrix F
Transition matrix that describes the evolution of the state.
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
vpMatrix H
Matrix that describes the evolution of the measurements.
bool verbose_mode
When set to true, print the content of internal variables during filtering() and prediction().
vpMatrix Q
Process noise covariance matrix .
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94