48#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
58 unsigned int nsignal = 2;
59 unsigned int niter = 200;
60 unsigned int size_state_vector = 2 * nsignal;
61 unsigned int size_measure_vector = 1 * nsignal;
63 vpMeasureType measure_t = Position;
65 std::string filename =
"/tmp/log.dat";
66 std::ofstream flog(filename.c_str());
71 for (
unsigned int signal = 0; signal < nsignal; signal++)
72 sigma_measure = 0.000001;
77 for (
unsigned int signal = 0; signal < nsignal; signal++) {
78 sigma_state[2 * signal] = 0.;
79 sigma_state[2 * signal + 1] = 0.000001;
83 for (
unsigned int signal = 0; signal < nsignal; signal++) {
84 sigma_state[2 * signal] = 0.000001;
85 sigma_state[2 * signal + 1] = 0;
92 for (
unsigned int signal = 0; signal < nsignal; signal++) {
93 measure[signal] = 3 + 2 * signal;
106 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
111 kalman.
initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
115 for (
unsigned int iter = 0; iter <= niter; iter++) {
116 std::cout <<
"-------- iter " << iter <<
" ------------" << std::endl;
117 for (
unsigned int signal = 0; signal < nsignal; signal++) {
118 measure[signal] = 3 + 2 * signal + 0.3 * sin(
vpMath::rad(360. / niter * iter));
120 std::cout <<
"measure : " << measure.t() << std::endl;
126 flog << kalman.
Xest.
t() << std::endl;
128 std::cout <<
"Xest: " << kalman.
Xest.
t() << std::endl;
134 std::cout <<
"Catch an exception: " << e << std::endl;
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)