49 #include <visp/vpLinearKalmanFilterInstantiation.h>
57 unsigned int nsignal = 1;
58 unsigned int niter = 100;
60 std::string filename =
"/tmp/log.dat";
61 std::ofstream flog(filename.c_str());
67 kalman.setStateModel(model);
69 unsigned int size_state_vector = kalman.getStateSize()*nsignal;
70 unsigned int size_measure_vector = kalman.getMeasureSize()*nsignal;
73 for (
unsigned int signal=0; signal < nsignal; signal ++)
74 sigma_measure = 0.0001;
76 for (
unsigned int signal=0; signal < nsignal; signal ++) {
77 sigma_state[3*signal] = 0.;
78 sigma_state[3*signal+1] = 0.000001;
79 sigma_state[3*signal+2] = 0.000001;
87 for (
unsigned int signal=0; signal < nsignal; signal ++)
88 velocity_measure[signal] = 3+2*signal;
90 kalman.verbose(
false);
91 kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
94 for (
unsigned int iter=0; iter <= niter; iter++) {
95 std::cout <<
"-------- iter " << iter <<
" ------------" << std::endl;
96 for (
unsigned int signal=0; signal < nsignal; signal ++) {
97 velocity_measure[signal] = 3+2*signal
100 std::cout <<
"measure : " << velocity_measure.t() << std::endl;
102 flog << velocity_measure.t();
105 kalman.filter(velocity_measure);
106 flog << kalman.Xest.t();
107 flog << kalman.Xpre.t();
109 std::cout <<
"Xest: " << kalman.Xest.t() << std::endl;
110 std::cout <<
"Xpre: " << kalman.Xpre.t() << std::endl;
119 std::cout <<
"Catch an exception: " << e << std::endl;
error that can be emited by ViSP classes.
static double rad(double deg)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
This class provides an implementation of some specific linear Kalman filters.