Visual Servoing Platform  version 3.6.1 under development (2024-07-15)
vpKalmanFilter.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
41 #include <visp3/core/vpKalmanFilter.h>
42 
43 #include <math.h>
44 #include <stdlib.h>
45 
58 void vpKalmanFilter::init(unsigned int size_state_vector, unsigned int size_measure_vector, unsigned int n_signal)
59 {
60  this->size_state = size_state_vector;
61  this->size_measure = size_measure_vector;
62  this->nsignal = n_signal;
65 
68 
70  Xest = 0;
72  Xpre = 0;
73 
75  Pest = 0;
76 
78  // init_done = false ;
79  iter = 0;
80  dt = -1;
81 }
82 
90  : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false), Xest(), Xpre(), F(), H(), R(), Q(),
91  dt(-1), Ppre(), Pest(), W(), I()
92 { }
93 
101 vpKalmanFilter::vpKalmanFilter(unsigned int n_signal)
102  : iter(0), size_state(0), size_measure(0), nsignal(n_signal), verbose_mode(false), Xest(), Xpre(), F(), H(), R(), Q(),
103  dt(-1), Ppre(), Pest(), W(), I()
104 { }
105 
119 vpKalmanFilter::vpKalmanFilter(unsigned int size_state_vector, unsigned int size_measure_vector, unsigned int n_signal)
120  : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false), Xest(), Xpre(), F(), H(), R(), Q(),
121  dt(-1), Ppre(), Pest(), W(), I()
122 {
123  init(size_state_vector, size_measure_vector, n_signal);
124 }
125 
143 {
144  if (Xest.getRows() != size_state * nsignal) {
145  throw(vpException(vpException::fatalError, "Error in vpKalmanFilter::prediction() %d != %d: Filter not initialized",
147  }
148 
149  if (verbose_mode) {
150  std::cout << "F = " << std::endl << F << std::endl;
151  std::cout << "Xest = " << std::endl << Xest << std::endl;
152  }
153  // Prediction
154  // Bar-Shalom 5.2.3.2
155  Xpre = F * Xest;
156  if (verbose_mode) {
157  std::cout << "Xpre = " << std::endl << Xpre << std::endl;
158  std::cout << "Q = " << std::endl << Q << std::endl;
159  std::cout << "Pest " << std::endl << Pest << std::endl;
160  }
161  // Bar-Shalom 5.2.3.5
162  Ppre = F * Pest * F.t() + Q;
163 
164  // Matrice de covariance de l'erreur de prediction
165  if (verbose_mode)
166  std::cout << "Ppre " << std::endl << Ppre << std::endl;
167 }
168 
200 {
201  if (verbose_mode)
202  std::cout << "z " << std::endl << z << std::endl;
203  // Bar-Shalom 5.2.3.11
204  vpMatrix S = H * Ppre * H.t() + R;
205  if (verbose_mode)
206  std::cout << "S " << std::endl << S << std::endl;
207 
208  W = (Ppre * H.t()) * (S).inverseByLU();
209  if (verbose_mode)
210  std::cout << "W " << std::endl << W << std::endl;
211  // Bar-Shalom 5.2.3.15
212  Pest = Ppre - W * S * W.t();
213  if (verbose_mode)
214  std::cout << "Pest " << std::endl << Pest << std::endl;
215 
216  if (0) {
217  // Bar-Shalom 5.2.3.16
218  // numeriquement plus stable que 5.2.3.15
219  vpMatrix Pestinv;
220  Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU() * H;
221  Pest = Pestinv.inverseByLU();
222  }
223  // Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
224  Xest = Xpre + (W * (z - (H * Xpre)));
225  if (verbose_mode)
226  std::cout << "Xest " << std::endl << Xest << std::endl;
227 
228  iter++;
229 }
230 
231 #if 0
232 
233 
282 void
283 vpKalmanFilter::initFilterCteAcceleration(double dt,
284  vpColVector &Z0,
285  vpColVector &Z1,
286  vpColVector &Z2,
287  vpColVector &sigma_noise,
288  vpColVector &sigma_state)
289 {
290  this->dt = dt;
291 
292  double dt2 = dt*dt;
293  double dt3 = dt2*dt;
294  double dt4 = dt3*dt;
295  double dt5 = dt4*dt;
296 
297  //init_done = true ;
298 
299  Pest = 0;
300  // initialise les matrices decrivant les modeles
301  for (int i = 0; i < size_measure; i++) {
302  // modele sur l'etat
303 
304  // | 1 dt dt2/2 |
305  // F = | 0 1 dt |
306  // | 0 0 1 |
307 
308  F[3*i][3*i] = 1;
309  F[3*i][3*i+1] = dt;
310  F[3*i][3*i+2] = dt*dt/2;
311  F[3*i+1][3*i+1] = 1;
312  F[3*i+1][3*i+2] = dt;
313  F[3*i+2][3*i+2] = 1;
314 
315 
316  // modele sur la mesure
317  H[i][3*i] = 1;
318  H[i][3*i+1] = 0;
319  H[i][3*i+2] = 0;
320 
321  double sR = sigma_noise[i];
322  double sQ = sigma_state[i];
323 
324  // bruit de mesure
325  R[i][i] = (sR);
326 
327  // bruit d'etat 6.2.3.9
328  Q[3*i][3*i] = sQ * dt5/20;
329  Q[3*i][3*i+1] = sQ * dt4/8;
330  Q[3*i][3*i+2] = sQ * dt3/6;
331 
332  Q[3*i+1][3*i] = sQ * dt4/8;
333  Q[3*i+1][3*i+1] = sQ * dt3/3;
334  Q[3*i+1][3*i+2] = sQ * dt2/2;
335 
336  Q[3*i+2][3*i] = sQ * dt3/6;
337  Q[3*i+2][3*i+1] = sQ * dt2/2.0;
338  Q[3*i+2][3*i+2] = sQ * dt;
339 
340 
341  // Initialisation pour la matrice de covariance sur l'etat
342 
343  Pest[3*i][3*i] = sR;
344  Pest[3*i][3*i+1] = 1.5/dt*sR;
345  Pest[3*i][3*i+2] = sR/(dt2);
346 
347  Pest[3*i+1][3*i] = 1.5/dt*sR;
348  Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR;
349  Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR;
350 
351  Pest[3*i+2][3*i] = sR/(dt2);
352  Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR;
353  Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR;
354 
355 
356  // Initialisation pour l'etat
357 
358  Xest[3*i] = Z2[i];
359  Xest[3*i+1] = (1.5 *Z2[i] - Z1[i] -0.5*Z0[i]) /(2*dt);
360  Xest[3*i+2] = (Z2[i] - 2*Z1[i] + Z0[i]) /(dt*dt);
361 
362  }
363 }
364 
365 void
366 vpKalmanFilter::initFilterSinger(double dt,
367  double a,
368  vpColVector &Z0,
369  vpColVector &Z1,
370  vpColVector &sigma_noise,
371  vpColVector &sigma_state)
372 {
373  this->dt = dt;
374 
375  double dt2 = dt*dt;
376  double dt3 = dt2*dt;
377 
378  double a2 = a*a;
379  double a3 = a2*a;
380  double a4 = a3*a;
381 
382  //init_done = true ;
383 
384  Pest = 0;
385  // initialise les matrices decrivant les modeles
386  for (int i = 0; i < size_measure; i++) {
387  // modele sur l'etat
388 
389  // | 1 dt dt2/2 |
390  // F = | 0 1 dt |
391  // | 0 0 1 |
392 
393  F[3*i][3*i] = 1;
394  F[3*i][3*i+1] = dt;
395  F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt));
396  F[3*i+1][3*i+1] = 1;
397  F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt));
398  F[3*i+2][3*i+2] = exp(-a*dt);
399 
400 
401  // modele sur la mesure
402  H[i][3*i] = 1;
403  H[i][3*i+1] = 0;
404  H[i][3*i+2] = 0;
405 
406  double sR = sigma_noise[i];
407  double sQ = sigma_state[i];
408 
409  R[i][i] = (sR); // bruit de mesure 1.5mm
410 
411  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));
412  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);
413  Q[3*i][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt));
414 
415  Q[3*i+1][3*i] = Q[3*i][3*i+1];
416  Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt);
417  Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt));
418 
419  Q[3*i+2][3*i] = Q[3*i][3*i+2];
420  Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2];
421  Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt));
422 
423 
424  // Initialisation pour la matrice de covariance sur l'etat
425  Pest[3*i][3*i] = sR;
426  Pest[3*i][3*i+1] = 1/dt*sR;
427  Pest[3*i][3*i+2] = 0;
428 
429  Pest[3*i+1][3*i] = 1/dt*sR;
430  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));
431  Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1);
432 
433  Pest[3*i+2][3*i] = 0;
434  Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2];
435  Pest[3*i+2][3*i+2] = 0;
436 
437 
438  // Initialisation pour l'etat
439 
440  Xest[3*i] = Z1[i];
441  Xest[3*i+1] = (Z1[i] - Z0[i]) /(dt);
442  Xest[3*i+2] = 0;
443 
444  }
445 }
446 
447 #endif
448 END_VISP_NAMESPACE
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
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 .
void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal)
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.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix inverseByLU() const
vpMatrix t() const