Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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;
66  F.resize(size_state * nsignal, size_state * nsignal);
67  H.resize(size_measure * nsignal, size_state * nsignal);
68 
69  R.resize(size_measure * nsignal, size_measure * nsignal);
70  Q.resize(size_state * nsignal, size_state * nsignal);
71 
72  Xest.resize(size_state * nsignal);
73  Xest = 0;
74  Xpre.resize(size_state * nsignal);
75  Xpre = 0;
76 
77  Pest.resize(size_state * nsignal, size_state * nsignal);
78  Pest = 0;
79 
80  I.resize(size_state * nsignal, size_state * nsignal);
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  std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows() << " " << size_state * nsignal << std::endl;
151  std::cout << " Error : Filter non initialized " << std::endl;
152  exit(1);
153  }
154 
155  // if (!init_done) {
156  // std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<"
157  // " << size_state<< std::endl ; std::cout << " Error : Filter non
158  // initialized " << std::endl; exit(1) ; return;
159  // }
160 
161  if (verbose_mode) {
162  std::cout << "F = " << std::endl << F << std::endl;
163  std::cout << "Xest = " << std::endl << Xest << std::endl;
164  }
165  // Prediction
166  // Bar-Shalom 5.2.3.2
167  Xpre = F * Xest;
168  if (verbose_mode) {
169  std::cout << "Xpre = " << std::endl << Xpre << std::endl;
170  std::cout << "Q = " << std::endl << Q << std::endl;
171  std::cout << "Pest " << std::endl << Pest << std::endl;
172  }
173  // Bar-Shalom 5.2.3.5
174  Ppre = F * Pest * F.t() + Q;
175 
176  // Matrice de covariance de l'erreur de prediction
177  if (verbose_mode)
178  std::cout << "Ppre " << std::endl << Ppre << std::endl;
179 }
180 
212 {
213  if (verbose_mode)
214  std::cout << "z " << std::endl << z << std::endl;
215  // Bar-Shalom 5.2.3.11
216  vpMatrix S = H * Ppre * H.t() + R;
217  if (verbose_mode)
218  std::cout << "S " << std::endl << S << std::endl;
219 
220  W = (Ppre * H.t()) * (S).inverseByLU();
221  if (verbose_mode)
222  std::cout << "W " << std::endl << W << std::endl;
223  // Bar-Shalom 5.2.3.15
224  Pest = Ppre - W * S * W.t();
225  if (verbose_mode)
226  std::cout << "Pest " << std::endl << Pest << std::endl;
227 
228  if (0) {
229  // Bar-Shalom 5.2.3.16
230  // numeriquement plus stable que 5.2.3.15
231  vpMatrix Pestinv;
232  Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU() * H;
233  Pest = Pestinv.inverseByLU();
234  }
235  // Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
236  Xest = Xpre + (W * (z - (H * Xpre)));
237  if (verbose_mode)
238  std::cout << "Xest " << std::endl << Xest << std::endl;
239 
240  iter++;
241 }
242 
243 #if 0
244 
245 
294 void
295 vpKalmanFilter::initFilterCteAcceleration(double dt,
296  vpColVector &Z0,
297  vpColVector &Z1,
298  vpColVector &Z2,
299  vpColVector &sigma_noise,
300  vpColVector &sigma_state )
301 {
302  this->dt = dt ;
303 
304  double dt2 = dt*dt ;
305  double dt3 = dt2*dt ;
306  double dt4 = dt3*dt ;
307  double dt5 = dt4*dt ;
308 
309  //init_done = true ;
310 
311  Pest =0 ;
312  // initialise les matrices decrivant les modeles
313  for (int i=0; i < size_measure ; i++ )
314  {
315  // modele sur l'etat
316 
317  // | 1 dt dt2/2 |
318  // F = | 0 1 dt |
319  // | 0 0 1 |
320 
321  F[3*i][3*i] = 1 ;
322  F[3*i][3*i+1] = dt ;
323  F[3*i][3*i+2] = dt*dt/2 ;
324  F[3*i+1][3*i+1] = 1 ;
325  F[3*i+1][3*i+2] = dt ;
326  F[3*i+2][3*i+2] = 1 ;
327 
328 
329  // modele sur la mesure
330  H[i][3*i] = 1 ;
331  H[i][3*i+1] = 0 ;
332  H[i][3*i+2] = 0 ;
333 
334  double sR = sigma_noise[i] ;
335  double sQ = sigma_state[i] ;
336 
337  // bruit de mesure
338  R[i][i] = (sR) ;
339 
340  // bruit d'etat 6.2.3.9
341  Q[3*i ][3*i ] = sQ * dt5/20;
342  Q[3*i ][3*i+1] = sQ * dt4/8;
343  Q[3*i ][3*i+2] = sQ * dt3/6 ;
344 
345  Q[3*i+1][3*i ] = sQ * dt4/8 ;
346  Q[3*i+1][3*i+1] = sQ * dt3/3 ;
347  Q[3*i+1][3*i+2] = sQ * dt2/2 ;
348 
349  Q[3*i+2][3*i ] = sQ * dt3/6 ;
350  Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
351  Q[3*i+2][3*i+2] = sQ * dt ;
352 
353 
354  // Initialisation pour la matrice de covariance sur l'etat
355 
356  Pest[3*i ][3*i ] = sR ;
357  Pest[3*i ][3*i+1] = 1.5/dt*sR ;
358  Pest[3*i ][3*i+2] = sR/(dt2) ;
359 
360  Pest[3*i+1][3*i ] = 1.5/dt*sR ;
361  Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
362  Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
363 
364  Pest[3*i+2][3*i ] = sR/(dt2) ;
365  Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
366  Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;
367 
368 
369  // Initialisation pour l'etat
370 
371  Xest[3*i] = Z2[i] ;
372  Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
373  Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;
374 
375  }
376 }
377 
378 void
379 vpKalmanFilter::initFilterSinger(double dt,
380  double a,
381  vpColVector &Z0,
382  vpColVector &Z1,
383  vpColVector &sigma_noise,
384  vpColVector &sigma_state )
385 {
386  this->dt = dt ;
387 
388  double dt2 = dt*dt ;
389  double dt3 = dt2*dt ;
390 
391  double a2 = a*a ;
392  double a3 = a2*a ;
393  double a4 = a3*a ;
394 
395  //init_done = true ;
396 
397  Pest =0 ;
398  // initialise les matrices decrivant les modeles
399  for (int i=0; i < size_measure ; i++ )
400  {
401  // modele sur l'etat
402 
403  // | 1 dt dt2/2 |
404  // F = | 0 1 dt |
405  // | 0 0 1 |
406 
407  F[3*i][3*i] = 1 ;
408  F[3*i][3*i+1] = dt ;
409  F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
410  F[3*i+1][3*i+1] = 1 ;
411  F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
412  F[3*i+2][3*i+2] = exp(-a*dt) ;
413 
414 
415  // modele sur la mesure
416  H[i][3*i] = 1 ;
417  H[i][3*i+1] = 0 ;
418  H[i][3*i+2] = 0 ;
419 
420  double sR = sigma_noise[i] ;
421  double sQ = sigma_state[i] ;
422 
423  R[i][i] = (sR) ; // bruit de mesure 1.5mm
424 
425  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) ) ;
426  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 ) ;
427  Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;
428 
429  Q[3*i+1][3*i ] = Q[3*i ][3*i+1] ;
430  Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt ) ;
431  Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;
432 
433  Q[3*i+2][3*i ] = Q[3*i ][3*i+2] ;
434  Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
435  Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;
436 
437 
438  // Initialisation pour la matrice de covariance sur l'etat
439  Pest[3*i ][3*i ] = sR ;
440  Pest[3*i ][3*i+1] = 1/dt*sR ;
441  Pest[3*i ][3*i+2] = 0 ;
442 
443  Pest[3*i+1][3*i ] = 1/dt*sR ;
444  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));
445  Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1) ;
446 
447  Pest[3*i+2][3*i ] = 0 ;
448  Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2] ;
449  Pest[3*i+2][3*i+2] = 0 ;
450 
451 
452  // Initialisation pour l'etat
453 
454  Xest[3*i] = Z1[i] ;
455  Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
456  Xest[3*i+2] = 0 ;
457 
458  }
459 }
460 
461 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
long iter
Filter step or iteration. When set to zero, initialize the filter.
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
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
Definition: vpArray2D.h:156
vpMatrix t() const
Definition: vpMatrix.cpp:375
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpMatrix inverseByLU() const
vpColVector Xpre
vpMatrix H
Matrix that describes the evolution of the measurements.
vpMatrix Q
Process noise covariance matrix .
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244