Visual Servoing Platform version 3.5.0
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
61void 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
105vpKalmanFilter::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
124vpKalmanFilter::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", Xest.getRows(), size_state * nsignal));
151 }
152
153 if (verbose_mode) {
154 std::cout << "F = " << std::endl << F << std::endl;
155 std::cout << "Xest = " << std::endl << Xest << std::endl;
156 }
157 // Prediction
158 // Bar-Shalom 5.2.3.2
159 Xpre = F * Xest;
160 if (verbose_mode) {
161 std::cout << "Xpre = " << std::endl << Xpre << std::endl;
162 std::cout << "Q = " << std::endl << Q << std::endl;
163 std::cout << "Pest " << std::endl << Pest << std::endl;
164 }
165 // Bar-Shalom 5.2.3.5
166 Ppre = F * Pest * F.t() + Q;
167
168 // Matrice de covariance de l'erreur de prediction
169 if (verbose_mode)
170 std::cout << "Ppre " << std::endl << Ppre << std::endl;
171}
172
204{
205 if (verbose_mode)
206 std::cout << "z " << std::endl << z << std::endl;
207 // Bar-Shalom 5.2.3.11
208 vpMatrix S = H * Ppre * H.t() + R;
209 if (verbose_mode)
210 std::cout << "S " << std::endl << S << std::endl;
211
212 W = (Ppre * H.t()) * (S).inverseByLU();
213 if (verbose_mode)
214 std::cout << "W " << std::endl << W << std::endl;
215 // Bar-Shalom 5.2.3.15
216 Pest = Ppre - W * S * W.t();
217 if (verbose_mode)
218 std::cout << "Pest " << std::endl << Pest << std::endl;
219
220 if (0) {
221 // Bar-Shalom 5.2.3.16
222 // numeriquement plus stable que 5.2.3.15
223 vpMatrix Pestinv;
224 Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU() * H;
225 Pest = Pestinv.inverseByLU();
226 }
227 // Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
228 Xest = Xpre + (W * (z - (H * Xpre)));
229 if (verbose_mode)
230 std::cout << "Xest " << std::endl << Xest << std::endl;
231
232 iter++;
233}
234
235#if 0
236
237
286void
287vpKalmanFilter::initFilterCteAcceleration(double dt,
288 vpColVector &Z0,
289 vpColVector &Z1,
290 vpColVector &Z2,
291 vpColVector &sigma_noise,
292 vpColVector &sigma_state )
293{
294 this->dt = dt ;
295
296 double dt2 = dt*dt ;
297 double dt3 = dt2*dt ;
298 double dt4 = dt3*dt ;
299 double dt5 = dt4*dt ;
300
301 //init_done = true ;
302
303 Pest =0 ;
304 // initialise les matrices decrivant les modeles
305 for (int i=0; i < size_measure ; i++ )
306 {
307 // modele sur l'etat
308
309 // | 1 dt dt2/2 |
310 // F = | 0 1 dt |
311 // | 0 0 1 |
312
313 F[3*i][3*i] = 1 ;
314 F[3*i][3*i+1] = dt ;
315 F[3*i][3*i+2] = dt*dt/2 ;
316 F[3*i+1][3*i+1] = 1 ;
317 F[3*i+1][3*i+2] = dt ;
318 F[3*i+2][3*i+2] = 1 ;
319
320
321 // modele sur la mesure
322 H[i][3*i] = 1 ;
323 H[i][3*i+1] = 0 ;
324 H[i][3*i+2] = 0 ;
325
326 double sR = sigma_noise[i] ;
327 double sQ = sigma_state[i] ;
328
329 // bruit de mesure
330 R[i][i] = (sR) ;
331
332 // bruit d'etat 6.2.3.9
333 Q[3*i ][3*i ] = sQ * dt5/20;
334 Q[3*i ][3*i+1] = sQ * dt4/8;
335 Q[3*i ][3*i+2] = sQ * dt3/6 ;
336
337 Q[3*i+1][3*i ] = sQ * dt4/8 ;
338 Q[3*i+1][3*i+1] = sQ * dt3/3 ;
339 Q[3*i+1][3*i+2] = sQ * dt2/2 ;
340
341 Q[3*i+2][3*i ] = sQ * dt3/6 ;
342 Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
343 Q[3*i+2][3*i+2] = sQ * dt ;
344
345
346 // Initialisation pour la matrice de covariance sur l'etat
347
348 Pest[3*i ][3*i ] = sR ;
349 Pest[3*i ][3*i+1] = 1.5/dt*sR ;
350 Pest[3*i ][3*i+2] = sR/(dt2) ;
351
352 Pest[3*i+1][3*i ] = 1.5/dt*sR ;
353 Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
354 Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
355
356 Pest[3*i+2][3*i ] = sR/(dt2) ;
357 Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
358 Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;
359
360
361 // Initialisation pour l'etat
362
363 Xest[3*i] = Z2[i] ;
364 Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
365 Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;
366
367 }
368}
369
370void
371vpKalmanFilter::initFilterSinger(double dt,
372 double a,
373 vpColVector &Z0,
374 vpColVector &Z1,
375 vpColVector &sigma_noise,
376 vpColVector &sigma_state )
377{
378 this->dt = dt ;
379
380 double dt2 = dt*dt ;
381 double dt3 = dt2*dt ;
382
383 double a2 = a*a ;
384 double a3 = a2*a ;
385 double a4 = a3*a ;
386
387 //init_done = true ;
388
389 Pest =0 ;
390 // initialise les matrices decrivant les modeles
391 for (int i=0; i < size_measure ; i++ )
392 {
393 // modele sur l'etat
394
395 // | 1 dt dt2/2 |
396 // F = | 0 1 dt |
397 // | 0 0 1 |
398
399 F[3*i][3*i] = 1 ;
400 F[3*i][3*i+1] = dt ;
401 F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
402 F[3*i+1][3*i+1] = 1 ;
403 F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
404 F[3*i+2][3*i+2] = exp(-a*dt) ;
405
406
407 // modele sur la mesure
408 H[i][3*i] = 1 ;
409 H[i][3*i+1] = 0 ;
410 H[i][3*i+2] = 0 ;
411
412 double sR = sigma_noise[i] ;
413 double sQ = sigma_state[i] ;
414
415 R[i][i] = (sR) ; // bruit de mesure 1.5mm
416
417 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) ) ;
418 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 ) ;
419 Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;
420
421 Q[3*i+1][3*i ] = Q[3*i ][3*i+1] ;
422 Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*dt ) ;
423 Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;
424
425 Q[3*i+2][3*i ] = Q[3*i ][3*i+2] ;
426 Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
427 Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;
428
429
430 // Initialisation pour la matrice de covariance sur l'etat
431 Pest[3*i ][3*i ] = sR ;
432 Pest[3*i ][3*i+1] = 1/dt*sR ;
433 Pest[3*i ][3*i+2] = 0 ;
434
435 Pest[3*i+1][3*i ] = 1/dt*sR ;
436 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));
437 Pest[3*i+1][3*i+2] = sQ/(a2*dt)*(exp(-a*dt)+a*dt-1) ;
438
439 Pest[3*i+2][3*i ] = 0 ;
440 Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2] ;
441 Pest[3*i+2][3*i+2] = 0 ;
442
443
444 // Initialisation pour l'etat
445
446 Xest[3*i] = Z1[i] ;
447 Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
448 Xest[3*i+2] = 0 ;
449
450 }
451}
452
453#endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:464