49 #include <visp/vpLinearKalmanFilterInstantiation.h>
61 unsigned int nsignal = 2;
62 unsigned int niter = 200;
63 unsigned int size_state_vector = 2*nsignal;
64 unsigned int size_measure_vector = 1*nsignal;
66 vpMeasureType measure_t = Position;
68 std::string filename =
"/tmp/log.dat";
69 std::ofstream flog(filename.c_str());
74 for (
unsigned int signal=0; signal < nsignal; signal ++)
75 sigma_measure = 0.000001;
80 for (
unsigned int signal=0; signal < nsignal; signal ++) {
81 sigma_state[2*signal] = 0.;
82 sigma_state[2*signal+1] = 0.000001;
86 for (
unsigned int signal=0; signal < nsignal; signal ++) {
87 sigma_state[2*signal] = 0.000001;
88 sigma_state[2*signal+1] = 0;
95 for (
unsigned int signal=0; signal < nsignal; signal ++) {
96 measure[signal] = 3+2*signal;
108 kalman.setStateModel(model);
109 kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
113 kalman.setStateModel(model);
114 kalman.initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
118 for (
unsigned int iter=0; iter <= niter; iter++) {
119 std::cout <<
"-------- iter " << iter <<
" ------------" << std::endl;
120 for (
unsigned int signal=0; signal < nsignal; signal ++) {
121 measure[signal] = 3+2*signal + 0.3*sin(
vpMath::rad(360./niter*iter));
123 std::cout <<
"measure : " << measure.t() << std::endl;
128 kalman.filter(measure);
129 flog << kalman.Xest.t() << std::endl;
131 std::cout <<
"Xest: " << kalman.Xest.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.