47 #include <visp/vpLinearKalmanFilterInstantiation.h>
48 #include <visp/vpDebug.h>
49 #include <visp/vpException.h>
180 double rho,
double dt)
193 sigma_measure, rho, dt);
319 double dt3 = dt2*
dt ;
329 F[2*i+1][2*i+1] = 1 ;
335 double sR = sigma_measure[i] ;
336 double sQ = sigma_state[2*i] ;
342 Q[2*i][2*i] = sQ * dt3/3;
343 Q[2*i][2*i+1] = sQ * dt2/2;
344 Q[2*i+1][2*i] = sQ * dt2/2;
345 Q[2*i+1][2*i+1] = sQ *
dt;
347 Pest[2*i][2*i] = sR ;
348 Pest[2*i][2*i+1] = sR/(2*
dt) ;
349 Pest[2*i+1][2*i] = sR/(2*
dt) ;
350 Pest[2*i+1][2*i+1] = sQ*2*dt/3.0+ sR/(2*dt2) ;
513 if ((rho < 0) || (rho >= 1)) {
538 F[2*i+1][2*i+1] = rho ;
544 double sR = sigma_measure[i] ;
545 double sQ = sigma_state[2*i+1] ;
554 Q[2*i+1][2*i+1] = sQ ;
556 Pest[2*i][2*i] = sR ;
557 Pest[2*i][2*i+1] = 0. ;
558 Pest[2*i+1][2*i] = 0 ;
559 Pest[2*i+1][2*i+1] = sQ/(1-rho*rho) ;
737 if ((rho < 0) || (rho >= 1)) {
763 F[3*i+1][3*i+1] = rho ;
764 F[3*i+2][3*i+2] = 1 ;
771 double sR = sigma_measure[i] ;
772 double sQ1 = sigma_state[3*i+1] ;
773 double sQ2 = sigma_state[3*i+2] ;
779 Q[3*i+1][3*i+1] = sQ1;
780 Q[3*i+2][3*i+2] = sQ2;
782 Pest[3*i][3*i] = sR ;
783 Pest[3*i][3*i+1] = 0. ;
784 Pest[3*i][3*i+2] = sR/
dt ;
785 Pest[3*i+1][3*i+1] = sQ1/(1-rho*rho) ;
786 Pest[3*i+1][3*i+2] = -rho*sQ1/((1-rho*rho)*dt) ;
787 Pest[3*i+2][3*i+2] = (2*sR+sQ1/(1-rho*rho))/(dt*
dt) ;
789 Pest[3*i+1][3*i] =
Pest[3*i][3*i+1];
790 Pest[3*i+2][3*i] =
Pest[3*i][3*i+2];
791 Pest[3*i+2][3*i+1] =
Pest[3*i+1][3*i+2];
812 vpERROR_TRACE(
"Bad signal number. You need to initialize the Kalman filter") ;
814 "Bad signal number")) ;
839 else if (
iter == 1) {