48 #include <visp/vpKalmanFilter.h>
65 unsigned int n_signal)
94 : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
95 Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
107 : iter(0), size_state(0), size_measure(0), nsignal(n_signal), verbose_mode(false),
108 Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
125 : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
126 Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
128 init( size_state_vector, size_measure_vector, n_signal) ;
150 std::cout <<
" in vpKalmanFilter::prediction()" <<
Xest.
getRows()
152 std::cout <<
" Error : Filter non initialized " << std::endl;
164 std::cout <<
"F = " << std::endl <<
F << std::endl ;
165 std::cout <<
"Xest = "<< std::endl <<
Xest << std::endl ;
171 std::cout <<
"Xpre = "<< std::endl <<
Xpre << std::endl ;
172 std::cout <<
"Q = "<< std::endl <<
Q << std::endl ;
173 std::cout <<
"Pest " << std::endl <<
Pest << std::endl ;
180 std::cout <<
"Ppre " << std::endl <<
Ppre << std::endl ;
218 std::cout <<
"z " << std::endl << z << std::endl ;
222 std::cout <<
"S " << std::endl << S << std::endl ;
226 std::cout <<
"W " << std::endl <<
W << std::endl ;
230 std::cout <<
"Pest " << std::endl <<
Pest << std::endl ;
242 std::cout <<
"Xest " << std::endl <<
Xest << std::endl ;
301 vpKalmanFilter::initFilterCteAcceleration(
double dt,
311 double dt3 = dt2*
dt ;
312 double dt4 = dt3*
dt ;
313 double dt5 = dt4*
dt ;
329 F[3*i][3*i+2] = dt*dt/2 ;
330 F[3*i+1][3*i+1] = 1 ;
331 F[3*i+1][3*i+2] =
dt ;
332 F[3*i+2][3*i+2] = 1 ;
340 double sR = sigma_noise[i] ;
341 double sQ = sigma_state[i] ;
347 Q[3*i ][3*i ] = sQ * dt5/20;
348 Q[3*i ][3*i+1] = sQ * dt4/8;
349 Q[3*i ][3*i+2] = sQ * dt3/6 ;
351 Q[3*i+1][3*i ] = sQ * dt4/8 ;
352 Q[3*i+1][3*i+1] = sQ * dt3/3 ;
353 Q[3*i+1][3*i+2] = sQ * dt2/2 ;
355 Q[3*i+2][3*i ] = sQ * dt3/6 ;
356 Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
357 Q[3*i+2][3*i+2] = sQ *
dt ;
362 Pest[3*i ][3*i ] = sR ;
363 Pest[3*i ][3*i+1] = 1.5/dt*sR ;
364 Pest[3*i ][3*i+2] = sR/(dt2) ;
366 Pest[3*i+1][3*i ] = 1.5/dt*sR ;
367 Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
368 Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
370 Pest[3*i+2][3*i ] = sR/(dt2) ;
371 Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
372 Pest[3*i+2][3*i+2] = 23*dt/30.0*sQ+6.0/dt4*sR ;
378 Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*dt ) ;
379 Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /( dt*dt ) ;
385 vpKalmanFilter::initFilterSinger(
double dt,
395 double dt3 = dt2*
dt ;
415 F[3*i][3*i+2] = 1/a2*(1+a*dt+exp(-a*dt)) ;
416 F[3*i+1][3*i+1] = 1 ;
417 F[3*i+1][3*i+2] = 1/a*(1-exp(-a*dt)) ;
418 F[3*i+2][3*i+2] = exp(-a*dt) ;
426 double sR = sigma_noise[i] ;
427 double sQ = sigma_state[i] ;
431 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) ) ;
432 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 ) ;
433 Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*dt)-2*a*dt*exp(-a*dt) ) ;
435 Q[3*i+1][3*i ] = Q[3*i ][3*i+1] ;
436 Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*dt)-3-exp(-2*a*dt)+2*a*
dt ) ;
437 Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*dt)+1- 2*exp(-a*dt)) ;
439 Q[3*i+2][3*i ] = Q[3*i ][3*i+2] ;
440 Q[3*i+2][3*i+1] = Q[3*i+1][3*i+2] ;
441 Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*dt) ) ;
445 Pest[3*i ][3*i ] = sR ;
446 Pest[3*i ][3*i+1] = 1/dt*sR ;
447 Pest[3*i ][3*i+2] = 0 ;
449 Pest[3*i+1][3*i ] = 1/dt*sR ;
450 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));
451 Pest[3*i+1][3*i+2] = sQ/(a2*
dt)*(exp(-a*dt)+a*dt-1) ;
453 Pest[3*i+2][3*i ] = 0 ;
454 Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2] ;
455 Pest[3*i+2][3*i+2] = 0 ;
461 Xest[3*i+1] = ( Z1[i] - Z0[i] ) /(dt ) ;
Definition of the vpMatrix class.
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
void filtering(vpColVector &z)
long iter
Filter step or iteration. When set to zero, initialize the filter.
unsigned int size_state
Size of the state vector .
vpMatrix R
Measurement noise covariance matrix .
unsigned int size_measure
Size of the measure vector .
vpMatrix I
Identity matrix .
unsigned int nsignal
Number of signal to filter.
void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
vpMatrix inverseByLU() const
vpMatrix F
Transition matrix that describes the evolution of the state.
unsigned int getRows() const
Return the number of rows of the matrix.
vpMatrix H
Matrix that describes the evolution of the measurements.
bool verbose_mode
When set to true, print the content of internal variables during filtering() and prediction().
vpMatrix Q
Process noise covariance matrix .
void resize(const unsigned int i, const bool flagNullify=true)