45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
175 vpColVector &sigma_measure,
double rho,
double delta_t)
305 double dt2 =
dt *
dt;
306 double dt3 = dt2 *
dt;
308 for (
unsigned int i = 0; i <
size_measure * n_signal; i++) {
315 F[2 * i][2 * i + 1] =
dt;
316 F[2 * i + 1][2 * i + 1] = 1;
322 double sR = sigma_measure[i];
323 double sQ = sigma_state[2 * i];
329 Q[2 * i][2 * i] = sQ * dt3 / 3;
330 Q[2 * i][2 * i + 1] = sQ * dt2 / 2;
331 Q[2 * i + 1][2 * i] = sQ * dt2 / 2;
332 Q[2 * i + 1][2 * i + 1] = sQ *
dt;
334 Pest[2 * i][2 * i] = sR;
335 Pest[2 * i][2 * i + 1] = sR / (2 *
dt);
336 Pest[2 * i + 1][2 * i] = sR / (2 *
dt);
337 Pest[2 * i + 1][2 * i + 1] = sQ * 2 *
dt / 3.0 + sR / (2 * dt2);
499 if ((rho < 0) || (rho >= 1)) {
516 for (
unsigned int i = 0; i <
size_measure * n_signal; i++) {
523 F[2 * i][2 * i + 1] = 1;
524 F[2 * i + 1][2 * i + 1] = rho;
530 double sR = sigma_measure[i];
531 double sQ = sigma_state[2 * i + 1];
538 Q[2 * i][2 * i + 1] = 0;
539 Q[2 * i + 1][2 * i] = 0;
540 Q[2 * i + 1][2 * i + 1] = sQ;
542 Pest[2 * i][2 * i] = sR;
543 Pest[2 * i][2 * i + 1] = 0.;
544 Pest[2 * i + 1][2 * i] = 0;
545 Pest[2 * i + 1][2 * i + 1] = sQ / (1 - rho * rho);
717 double rho,
double delta_t)
719 if ((rho < 0) || (rho >= 1)) {
743 F[3 * i][3 * i + 1] = 1;
744 F[3 * i][3 * i + 2] =
dt;
745 F[3 * i + 1][3 * i + 1] = rho;
746 F[3 * i + 2][3 * i + 2] = 1;
753 double sR = sigma_measure[i];
754 double sQ1 = sigma_state[3 * i + 1];
755 double sQ2 = sigma_state[3 * i + 2];
761 Q[3 * i + 1][3 * i + 1] = sQ1;
762 Q[3 * i + 2][3 * i + 2] = sQ2;
764 Pest[3 * i][3 * i] = sR;
765 Pest[3 * i][3 * i + 1] = 0.;
766 Pest[3 * i][3 * i + 2] = sR /
dt;
767 Pest[3 * i + 1][3 * i + 1] = sQ1 / (1 - rho * rho);
768 Pest[3 * i + 1][3 * i + 2] = -rho * sQ1 / ((1 - rho * rho) *
dt);
769 Pest[3 * i + 2][3 * i + 2] = (2 * sR + sQ1 / (1 - rho * rho)) / (
dt *
dt);
771 Pest[3 * i + 1][3 * i] =
Pest[3 * i][3 * i + 1];
772 Pest[3 * i + 2][3 * i] =
Pest[3 * i][3 * i + 2];
773 Pest[3 * i + 2][3 * i + 1] =
Pest[3 * i + 1][3 * i + 2];
793 vpERROR_TRACE(
"Bad signal number. You need to initialize the Kalman filter");
818 }
else if (
iter == 1) {