41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
171 vpColVector &sigma_measure,
double rho,
double delta_t)
301 double dt2 =
dt *
dt;
302 double dt3 = dt2 *
dt;
304 for (
unsigned int i = 0; i <
size_measure * n_signal; i++) {
311 F[2 * i][2 * i + 1] =
dt;
312 F[2 * i + 1][2 * i + 1] = 1;
318 double sR = sigma_measure[i];
319 double sQ = sigma_state[2 * i];
325 Q[2 * i][2 * i] = sQ * dt3 / 3;
326 Q[2 * i][2 * i + 1] = sQ * dt2 / 2;
327 Q[2 * i + 1][2 * i] = sQ * dt2 / 2;
328 Q[2 * i + 1][2 * i + 1] = sQ *
dt;
330 Pest[2 * i][2 * i] = sR;
331 Pest[2 * i][2 * i + 1] = sR / (2 *
dt);
332 Pest[2 * i + 1][2 * i] = sR / (2 *
dt);
333 Pest[2 * i + 1][2 * i + 1] = sQ * 2 *
dt / 3.0 + sR / (2 * dt2);
495 if ((rho < 0) || (rho >= 1)) {
512 for (
unsigned int i = 0; i <
size_measure * n_signal; i++) {
519 F[2 * i][2 * i + 1] = 1;
520 F[2 * i + 1][2 * i + 1] = rho;
526 double sR = sigma_measure[i];
527 double sQ = sigma_state[2 * i + 1];
534 Q[2 * i][2 * i + 1] = 0;
535 Q[2 * i + 1][2 * i] = 0;
536 Q[2 * i + 1][2 * i + 1] = sQ;
538 Pest[2 * i][2 * i] = sR;
539 Pest[2 * i][2 * i + 1] = 0.;
540 Pest[2 * i + 1][2 * i] = 0;
541 Pest[2 * i + 1][2 * i + 1] = sQ / (1 - rho * rho);
713 double rho,
double delta_t)
715 if ((rho < 0) || (rho >= 1)) {
739 F[3 * i][3 * i + 1] = 1;
740 F[3 * i][3 * i + 2] =
dt;
741 F[3 * i + 1][3 * i + 1] = rho;
742 F[3 * i + 2][3 * i + 2] = 1;
749 double sR = sigma_measure[i];
750 double sQ1 = sigma_state[3 * i + 1];
751 double sQ2 = sigma_state[3 * i + 2];
757 Q[3 * i + 1][3 * i + 1] = sQ1;
758 Q[3 * i + 2][3 * i + 2] = sQ2;
760 Pest[3 * i][3 * i] = sR;
761 Pest[3 * i][3 * i + 1] = 0.;
762 Pest[3 * i][3 * i + 2] = sR /
dt;
763 Pest[3 * i + 1][3 * i + 1] = sQ1 / (1 - rho * rho);
764 Pest[3 * i + 1][3 * i + 2] = -rho * sQ1 / ((1 - rho * rho) *
dt);
765 Pest[3 * i + 2][3 * i + 2] = (2 * sR + sQ1 / (1 - rho * rho)) / (
dt *
dt);
767 Pest[3 * i + 1][3 * i] =
Pest[3 * i][3 * i + 1];
768 Pest[3 * i + 2][3 * i] =
Pest[3 * i][3 * i + 2];
769 Pest[3 * i + 2][3 * i + 1] =
Pest[3 * i + 1][3 * i + 2];
789 vpERROR_TRACE(
"Bad signal number. You need to initialize the Kalman filter");
813 }
else if (
iter == 1) {
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ notInitialized
Used to indicate that a parameter is not initialized.
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 H
Matrix that describes the evolution of the measurements.
void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal)
void initStateConstVel_MeasurePos(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double dt)
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void filter(vpColVector &z)
void setStateModel(vpStateModel model)
void initStateConstAccWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
@ stateConstVel_MeasurePos
@ stateConstVelWithColoredNoise_MeasureVel
@ stateConstAccWithColoredNoise_MeasureVel
void initStateConstVelWithColoredNoise_MeasureVel(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho)