49 #include <visp/vpLinearKalmanFilterInstantiation.h>
62 unsigned int nsignal = 2;
63 unsigned int niter = 200;
64 unsigned int size_state_vector = 2*nsignal;
65 unsigned int size_measure_vector = 1*nsignal;
67 vpMeasureType measure_t = Position;
69 std::string filename =
"/tmp/log.dat";
70 std::ofstream flog(filename.c_str());
75 for (
unsigned int signal=0; signal < nsignal; signal ++)
76 sigma_measure = 0.000001;
81 for (
unsigned int signal=0; signal < nsignal; signal ++) {
82 sigma_state[2*signal] = 0.;
83 sigma_state[2*signal+1] = 0.000001;
87 for (
unsigned int signal=0; signal < nsignal; signal ++) {
88 sigma_state[2*signal] = 0.000001;
89 sigma_state[2*signal+1] = 0;
96 for (
unsigned int signal=0; signal < nsignal; signal ++) {
97 measure[signal] = 3+2*signal;
100 kalman.verbose(
true);
109 kalman.setStateModel(model);
110 kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
114 kalman.setStateModel(model);
115 kalman.initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
119 for (
unsigned int iter=0; iter <= niter; iter++) {
120 std::cout <<
"-------- iter " << iter <<
" ------------" << std::endl;
121 for (
unsigned int signal=0; signal < nsignal; signal ++) {
122 measure[signal] = 3+2*signal + 0.3*sin(
vpMath::rad(360./niter*iter));
124 std::cout <<
"measure : " << measure.t() << std::endl;
129 kalman.filter(measure);
130 flog << kalman.Xest.t() << std::endl;
132 std::cout <<
"Xest: " << kalman.Xest.t() << std::endl;
139 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.