48 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
53 unsigned int nsignal = 1;
54 unsigned int niter = 100;
56 std::string filename =
"/tmp/log.dat";
57 std::ofstream flog(filename.c_str());
65 unsigned int size_state_vector = kalman.
getStateSize() * nsignal;
66 unsigned int size_measure_vector = kalman.
getMeasureSize() * nsignal;
69 for (
unsigned int signal = 0; signal < nsignal; signal++)
70 sigma_measure = 0.0001;
72 for (
unsigned int signal = 0; signal < nsignal; signal++) {
73 sigma_state[3 * signal] = 0.;
74 sigma_state[3 * signal + 1] = 0.000001;
75 sigma_state[3 * signal + 2] = 0.000001;
83 for (
unsigned int signal = 0; signal < nsignal; signal++)
84 velocity_measure[signal] = 3 + 2 * signal;
87 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
89 for (
unsigned int iter = 0; iter <= niter; iter++) {
90 std::cout <<
"-------- iter " << iter <<
" ------------" << std::endl;
91 for (
unsigned int signal = 0; signal < nsignal; signal++) {
92 velocity_measure[signal] = 3 + 2 * signal + 0.3 * sin(
vpMath::rad(360. / niter * iter));
94 std::cout <<
"measure : " << velocity_measure.t() << std::endl;
96 flog << velocity_measure.t();
99 kalman.
filter(velocity_measure);
100 flog << kalman.
Xest.
t();
101 flog << kalman.
Xpre.
t();
103 std::cout <<
"Xest: " << kalman.
Xest.
t() << std::endl;
104 std::cout <<
"Xpre: " << kalman.
Xpre.
t() << std::endl;
112 std::cout <<
"Catch an exception: " << e << std::endl;