45 #include <visp3/core/vpKalmanFilter.h>
61 void vpKalmanFilter::init(
unsigned int size_state_vector,
unsigned int size_measure_vector,
unsigned int n_signal)
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()
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()
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()
128 init(size_state_vector, size_measure_vector, n_signal);
155 std::cout <<
"F = " << std::endl <<
F << std::endl;
156 std::cout <<
"Xest = " << std::endl <<
Xest << std::endl;
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;
171 std::cout <<
"Ppre " << std::endl <<
Ppre << std::endl;
207 std::cout <<
"z " << std::endl << z << std::endl;
211 std::cout <<
"S " << std::endl << S << std::endl;
215 std::cout <<
"W " << std::endl <<
W << std::endl;
219 std::cout <<
"Pest " << std::endl <<
Pest << std::endl;
231 std::cout <<
"Xest " << std::endl <<
Xest << std::endl;
288 vpKalmanFilter::initFilterCteAcceleration(
double dt,
298 double dt3 = dt2*
dt ;
299 double dt4 = dt3*
dt ;
300 double dt5 = dt4*
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 ;
327 double sR = sigma_noise[i] ;
328 double sQ = sigma_state[i] ;
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 ;
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 ;
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 ;
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) ;
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 ;
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 ;
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 ) ;
372 vpKalmanFilter::initFilterSinger(
double dt,
382 double dt3 = dt2*
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) ;
413 double sR = sigma_noise[i] ;
414 double sQ = sigma_state[i] ;
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) ) ;
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)) ;
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) ) ;
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 ;
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) ;
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 ;
448 Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(
dt ) ;
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int getRows() const
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emited by ViSP classes.
unsigned int size_state
Size of the state vector .
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)
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.
vpMatrix inverseByLU() const