Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpKalmanFilter.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Kalman filtering.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 
45 #include <visp3/core/vpKalmanFilter.h>
46 
47 #include <math.h>
48 #include <stdlib.h>
49 
60 void
61 vpKalmanFilter::init(unsigned int size_state_vector, unsigned int size_measure_vector,
62  unsigned int n_signal)
63 {
64  this->size_state = size_state_vector;
65  this->size_measure = size_measure_vector ;
66  this->nsignal = n_signal ;
68  H.resize(size_measure*nsignal, size_state*nsignal) ;
69 
70  R.resize(size_measure*nsignal, size_measure*nsignal) ;
71  Q.resize(size_state*nsignal, size_state*nsignal) ;
72 
73  Xest.resize(size_state*nsignal) ; Xest = 0;
74  Xpre.resize(size_state*nsignal) ; Xpre = 0 ;
75 
76  Pest.resize(size_state*nsignal, size_state*nsignal) ; Pest = 0 ;
77 
78  I.resize(size_state*nsignal, size_state*nsignal) ;
79  // init_done = false ;
80  iter = 0 ;
81  dt = -1 ;
82 }
83 
91  : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
92  Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
93 {
94 }
95 
103 vpKalmanFilter::vpKalmanFilter(unsigned int n_signal)
104  : iter(0), size_state(0), size_measure(0), nsignal(n_signal), verbose_mode(false),
105  Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
106 {
107 }
108 
121 vpKalmanFilter::vpKalmanFilter(unsigned int size_state_vector, unsigned int size_measure_vector, unsigned int n_signal)
122  : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
123  Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
124 {
125  init( size_state_vector, size_measure_vector, n_signal) ;
126 }
127 
143 void
145 {
146  if (Xest.getRows() != size_state*nsignal) {
147  std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()
148  <<" " << size_state*nsignal<< std::endl ;
149  std::cout << " Error : Filter non initialized " << std::endl;
150  exit(1) ;
151  }
152 
153 // if (!init_done) {
154 // std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<" " << size_state<< std::endl ;
155 // std::cout << " Error : Filter non initialized " << std::endl;
156 // exit(1) ;
157 // return;
158 // }
159 
160  if (verbose_mode) {
161  std::cout << "F = " << std::endl << F << std::endl ;
162  std::cout << "Xest = "<< std::endl << Xest << std::endl ;
163  }
164  // Prediction
165  // Bar-Shalom 5.2.3.2
166  Xpre = F*Xest ;
167  if (verbose_mode) {
168  std::cout << "Xpre = "<< std::endl << Xpre << std::endl ;
169  std::cout << "Q = "<< std::endl << Q << std::endl ;
170  std::cout << "Pest " << std::endl << Pest << std::endl ;
171  }
172  // Bar-Shalom 5.2.3.5
173  Ppre = F*Pest*F.t() + Q ;
174 
175  // Matrice de covariance de l'erreur de prediction
176  if (verbose_mode)
177  std::cout << "Ppre " << std::endl << Ppre << std::endl ;
178 }
179 
211 void
213 {
214  if (verbose_mode)
215  std::cout << "z " << std::endl << z << std::endl ;
216  // Bar-Shalom 5.2.3.11
217  vpMatrix S = H*Ppre*H.t() + R ;
218  if (verbose_mode)
219  std::cout << "S " << std::endl << S << std::endl ;
220 
221  W = (Ppre * H.t())* (S).inverseByLU() ;
222  if (verbose_mode)
223  std::cout << "W " << std::endl << W << std::endl ;
224  // Bar-Shalom 5.2.3.15
225  Pest = Ppre - W*S*W.t() ;
226  if (verbose_mode)
227  std::cout << "Pest " << std::endl << Pest << std::endl ;
228 
229  if (0) {
230  // Bar-Shalom 5.2.3.16
231  // numeriquement plus stable que 5.2.3.15
232  vpMatrix Pestinv ;
233  Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU()*H ;
234  Pest = Pestinv.inverseByLU() ;
235  }
236  // Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
237  Xest = Xpre + (W*(z - (H*Xpre))) ;
238  if (verbose_mode)
239  std::cout << "Xest " << std::endl << Xest << std::endl ;
240 
241  iter++ ;
242 }
243 
244 
245 #if 0
246 
247 
296 void
297 vpKalmanFilter::initFilterCteAcceleration(double dt,
298  vpColVector &Z0,
299  vpColVector &Z1,
300  vpColVector &Z2,
301  vpColVector &sigma_noise,
302  vpColVector &sigma_state )
303 {
304  this->dt = dt ;
305 
306  double dt2 = dt*dt ;
307  double dt3 = dt2*dt ;
308  double dt4 = dt3*dt ;
309  double dt5 = dt4*dt ;
310 
311  //init_done = true ;
312 
313  Pest =0 ;
314  // initialise les matrices decrivant les modeles
315  for (int i=0; i < size_measure ; i++ )
316  {
317  // modele sur l'etat
318 
319  // | 1 dt dt2/2 |
320  // F = | 0 1 dt |
321  // | 0 0 1 |
322 
323  F[3*i][3*i] = 1 ;
324  F[3*i][3*i+1] = dt ;
325  F[3*i][3*i+2] = dt*dt/2 ;
326  F[3*i+1][3*i+1] = 1 ;
327  F[3*i+1][3*i+2] = dt ;
328  F[3*i+2][3*i+2] = 1 ;
329 
330 
331  // modele sur la mesure
332  H[i][3*i] = 1 ;
333  H[i][3*i+1] = 0 ;
334  H[i][3*i+2] = 0 ;
335 
336  double sR = sigma_noise[i] ;
337  double sQ = sigma_state[i] ;
338 
339  // bruit de mesure
340  R[i][i] = (sR) ;
341 
342  // bruit d'etat 6.2.3.9
343  Q[3*i ][3*i ] = sQ * dt5/20;
344  Q[3*i ][3*i+1] = sQ * dt4/8;
345  Q[3*i ][3*i+2] = sQ * dt3/6 ;
346 
347  Q[3*i+1][3*i ] = sQ * dt4/8 ;
348  Q[3*i+1][3*i+1] = sQ * dt3/3 ;
349  Q[3*i+1][3*i+2] = sQ * dt2/2 ;
350 
351  Q[3*i+2][3*i ] = sQ * dt3/6 ;
352  Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
353  Q[3*i+2][3*i+2] = sQ * dt ;
354 
355 
356  // Initialisation pour la matrice de covariance sur l'etat
357 
358  Pest[3*i ][3*i ] = sR ;
359  Pest[3*i ][3*i+1] = 1.5/dt*sR ;
360  Pest[3*i ][3*i+2] = sR/(dt2) ;
361 
362  Pest[3*i+1][3*i ] = 1.5/dt*sR ;
363  Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
364  Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
365 
366  Pest[3*i+2][3*i ] = sR/(dt2) ;
367  Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
368  Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;
369 
370 
371  // Initialisation pour l'etat
372 
373  Xest[3*i] = Z2[i] ;
374  Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
375  Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;
376 
377  }
378 }
379 
380 void
381 vpKalmanFilter::initFilterSinger(double dt,
382  double a,
383  vpColVector &Z0,
384  vpColVector &Z1,
385  vpColVector &sigma_noise,
386  vpColVector &sigma_state )
387 {
388  this->dt = dt ;
389 
390  double dt2 = dt*dt ;
391  double dt3 = dt2*dt ;
392 
393  double a2 = a*a ;
394  double a3 = a2*a ;
395  double a4 = a3*a ;
396 
397  //init_done = true ;
398 
399  Pest =0 ;
400  // initialise les matrices decrivant les modeles
401  for (int i=0; i < size_measure ; i++ )
402  {
403  // modele sur l'etat
404 
405  // | 1 dt dt2/2 |
406  // F = | 0 1 dt |
407  // | 0 0 1 |
408 
409  F[3*i][3*i] = 1 ;
410  F[3*i][3*i+1] = dt ;
411  F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
412  F[3*i+1][3*i+1] = 1 ;
413  F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
414  F[3*i+2][3*i+2] = exp(-a*dt) ;
415 
416 
417  // modele sur la mesure
418  H[i][3*i] = 1 ;
419  H[i][3*i+1] = 0 ;
420  H[i][3*i+2] = 0 ;
421 
422  double sR = sigma_noise[i] ;
423  double sQ = sigma_state[i] ;
424 
425  R[i][i] = (sR) ; // bruit de mesure 1.5mm
426 
427  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) ) ;
428  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 ) ;
429  Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;
430 
431  Q[3*i+1][3*i ] = Q[3*i ][3*i+1] ;
432  Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt ) ;
433  Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;
434 
435  Q[3*i+2][3*i ] = Q[3*i ][3*i+2] ;
436  Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
437  Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;
438 
439 
440  // Initialisation pour la matrice de covariance sur l'etat
441  Pest[3*i ][3*i ] = sR ;
442  Pest[3*i ][3*i+1] = 1/dt*sR ;
443  Pest[3*i ][3*i+2] = 0 ;
444 
445  Pest[3*i+1][3*i ] = 1/dt*sR ;
446  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));
447  Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1) ;
448 
449  Pest[3*i+2][3*i ] = 0 ;
450  Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2] ;
451  Pest[3*i+2][3*i+2] = 0 ;
452 
453 
454  // Initialisation pour l'etat
455 
456  Xest[3*i] = Z1[i] ;
457  Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
458  Xest[3*i+2] = 0 ;
459 
460  }
461 }
462 
463 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
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 .
void filtering(const vpColVector &z)
unsigned int nsignal
Number of signal to filter.
void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal)
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
vpMatrix t() const
Definition: vpMatrix.cpp:207
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
vpColVector Xpre
vpMatrix F
Transition matrix that describes the evolution of the state.
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:225