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