#include <fstream>
#include <iostream>
#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
typedef enum {
Position,
Velocity
} vpMeasureType;
int main()
{
try {
unsigned int nsignal = 2;
unsigned int niter = 200;
unsigned int size_state_vector = 2 * nsignal;
unsigned int size_measure_vector = 1 * nsignal;
vpMeasureType measure_t = Position;
std::string filename = "/tmp/log.dat";
std::ofstream flog(filename.c_str());
for (unsigned int signal = 0; signal < nsignal; signal++)
sigma_measure = 0.000001;
switch (measure_t) {
case Velocity:
for (unsigned int signal = 0; signal < nsignal; signal++) {
sigma_state[2 * signal] = 0.;
sigma_state[2 * signal + 1] = 0.000001;
}
break;
case Position:
for (unsigned int signal = 0; signal < nsignal; signal++) {
sigma_state[2 * signal] = 0.000001;
sigma_state[2 * signal + 1] = 0;
}
break;
}
for (unsigned int signal = 0; signal < nsignal; signal++) {
measure[signal] = 3 + 2 * signal;
}
double dt = 0.04;
double rho = 0.5;
double dummy = 0;
switch (measure_t) {
case Velocity:
kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
break;
case Position:
kalman.
initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
break;
}
for (unsigned int iter = 0; iter <= niter; iter++) {
std::cout << "-------- iter " << iter << " ------------" << std::endl;
for (unsigned int signal = 0; signal < nsignal; signal++) {
measure[signal] = 3 + 2 * signal + 0.3 * sin(
vpMath::rad(360. / niter * iter));
}
std::cout << "measure : " << measure.t() << std::endl;
flog << measure.t();
flog << kalman.
Xest.
t() << std::endl;
std::cout <<
"Xest: " << kalman.
Xest.
t() << std::endl;
}
flog.close();
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
This class provides an implementation of some specific linear Kalman filters.
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void filter(vpColVector &z)
void setStateModel(vpStateModel model)
@ stateConstVel_MeasurePos
@ stateConstVelWithColoredNoise_MeasureVel
static double rad(double deg)