49 #include <visp/vpLinearKalmanFilterInstantiation.h>
56 unsigned int nsignal = 1;
57 unsigned int niter = 100;
59 std::string filename =
"/tmp/log.dat";
60 std::ofstream flog(filename.c_str());
66 kalman.setStateModel(model);
68 unsigned int size_state_vector = kalman.getStateSize()*nsignal;
69 unsigned int size_measure_vector = kalman.getMeasureSize()*nsignal;
72 for (
unsigned int signal=0; signal < nsignal; signal ++)
73 sigma_measure = 0.0001;
75 for (
unsigned int signal=0; signal < nsignal; signal ++) {
76 sigma_state[3*signal] = 0.;
77 sigma_state[3*signal+1] = 0.000001;
78 sigma_state[3*signal+2] = 0.000001;
86 for (
unsigned int signal=0; signal < nsignal; signal ++)
87 velocity_measure[signal] = 3+2*signal;
89 kalman.verbose(
false);
90 kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
93 for (
unsigned int iter=0; iter <= niter; iter++) {
94 std::cout <<
"-------- iter " << iter <<
" ------------" << std::endl;
95 for (
unsigned int signal=0; signal < nsignal; signal ++) {
96 velocity_measure[signal] = 3+2*signal
99 std::cout <<
"measure : " << velocity_measure.t() << std::endl;
101 flog << velocity_measure.t();
104 kalman.filter(velocity_measure);
105 flog << kalman.Xest.t();
106 flog << kalman.Xpre.t();
108 std::cout <<
"Xest: " << kalman.Xest.t() << std::endl;
109 std::cout <<
"Xpre: " << kalman.Xpre.t() << std::endl;
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.