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;