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