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);
151 std::cout <<
" Error : Filter non initialized " << std::endl;
162 std::cout <<
"F = " << std::endl <<
F << std::endl;
163 std::cout <<
"Xest = " << std::endl <<
Xest << std::endl;
169 std::cout <<
"Xpre = " << std::endl <<
Xpre << std::endl;
170 std::cout <<
"Q = " << std::endl <<
Q << std::endl;
171 std::cout <<
"Pest " << std::endl <<
Pest << std::endl;
178 std::cout <<
"Ppre " << std::endl <<
Ppre << std::endl;
214 std::cout <<
"z " << std::endl << z << std::endl;
218 std::cout <<
"S " << std::endl << S << std::endl;
222 std::cout <<
"W " << std::endl <<
W << std::endl;
226 std::cout <<
"Pest " << std::endl <<
Pest << std::endl;
238 std::cout <<
"Xest " << std::endl <<
Xest << std::endl;
295 vpKalmanFilter::initFilterCteAcceleration(
double dt,
305 double dt3 = dt2*
dt ;
306 double dt4 = dt3*
dt ;
307 double dt5 = dt4*
dt ;
323 F[3*i][3*i+2] =
dt*
dt/2 ;
324 F[3*i+1][3*i+1] = 1 ;
325 F[3*i+1][3*i+2] =
dt ;
326 F[3*i+2][3*i+2] = 1 ;
334 double sR = sigma_noise[i] ;
335 double sQ = sigma_state[i] ;
341 Q[3*i ][3*i ] = sQ * dt5/20;
342 Q[3*i ][3*i+1] = sQ * dt4/8;
343 Q[3*i ][3*i+2] = sQ * dt3/6 ;
345 Q[3*i+1][3*i ] = sQ * dt4/8 ;
346 Q[3*i+1][3*i+1] = sQ * dt3/3 ;
347 Q[3*i+1][3*i+2] = sQ * dt2/2 ;
349 Q[3*i+2][3*i ] = sQ * dt3/6 ;
350 Q[3*i+2][3*i+1] = sQ * dt2/2.0 ;
351 Q[3*i+2][3*i+2] = sQ *
dt ;
356 Pest[3*i ][3*i ] = sR ;
357 Pest[3*i ][3*i+1] = 1.5/
dt*sR ;
358 Pest[3*i ][3*i+2] = sR/(dt2) ;
360 Pest[3*i+1][3*i ] = 1.5/
dt*sR ;
361 Pest[3*i+1][3*i+1] = dt3/3*sQ + 13/(2*dt2)*sR ;
362 Pest[3*i+1][3*i+2] = 9*dt2*sQ/40.0 +6/dt3*sR ;
364 Pest[3*i+2][3*i ] = sR/(dt2) ;
365 Pest[3*i+2][3*i+1] = 9*dt2*sQ/40.0 +6/dt3*sR ;
366 Pest[3*i+2][3*i+2] = 23*
dt/30.0*sQ+6.0/dt4*sR ;
372 Xest[3*i+1] = ( 1.5 *Z2[i] - Z1[i] -0.5*Z0[i] ) /( 2*
dt ) ;
373 Xest[3*i+2] = ( Z2[i] - 2*Z1[i] + Z0[i] ) /(
dt*
dt ) ;
379 vpKalmanFilter::initFilterSinger(
double dt,
389 double dt3 = dt2*
dt ;
409 F[3*i][3*i+2] = 1/a2*(1+a*
dt+exp(-a*
dt)) ;
410 F[3*i+1][3*i+1] = 1 ;
411 F[3*i+1][3*i+2] = 1/a*(1-exp(-a*
dt)) ;
412 F[3*i+2][3*i+2] = exp(-a*
dt) ;
420 double sR = sigma_noise[i] ;
421 double sQ = sigma_state[i] ;
425 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) ) ;
426 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 ) ;
427 Q[3*i ][3*i+2] = sQ/a2*(1-exp(-2*a*
dt)-2*a*
dt*exp(-a*
dt) ) ;
429 Q[3*i+1][3*i ] =
Q[3*i ][3*i+1] ;
430 Q[3*i+1][3*i+1] = sQ/a2*(4*exp(-a*
dt)-3-exp(-2*a*
dt)+2*a*
dt ) ;
431 Q[3*i+1][3*i+2] = sQ/a*(exp(-2*a*
dt)+1- 2*exp(-a*
dt)) ;
433 Q[3*i+2][3*i ] =
Q[3*i ][3*i+2] ;
434 Q[3*i+2][3*i+1] =
Q[3*i+1][3*i+2] ;
435 Q[3*i+2][3*i+2] = sQ*(1-exp(-2*a*
dt) ) ;
439 Pest[3*i ][3*i ] = sR ;
440 Pest[3*i ][3*i+1] = 1/
dt*sR ;
441 Pest[3*i ][3*i+2] = 0 ;
443 Pest[3*i+1][3*i ] = 1/
dt*sR ;
444 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));
445 Pest[3*i+1][3*i+2] = sQ/(a2*
dt)*(exp(-a*
dt)+a*
dt-1) ;
447 Pest[3*i+2][3*i ] = 0 ;
448 Pest[3*i+2][3*i+1] =
Pest[3*i+1][3*i+2] ;
449 Pest[3*i+2][3*i+2] = 0 ;
455 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)
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