45 #include <visp3/core/vpLinearKalmanFilterInstantiation.h>
55 unsigned int nsignal = 2;
56 unsigned int niter = 200;
57 unsigned int size_state_vector = 2 * nsignal;
58 unsigned int size_measure_vector = 1 * nsignal;
60 vpMeasureType measure_t = Position;
62 std::string filename =
"/tmp/log.dat";
63 std::ofstream flog(filename.c_str());
68 for (
unsigned int signal = 0; signal < nsignal; signal++)
69 sigma_measure = 0.000001;
74 for (
unsigned int signal = 0; signal < nsignal; signal++) {
75 sigma_state[2 * signal] = 0.;
76 sigma_state[2 * signal + 1] = 0.000001;
80 for (
unsigned int signal = 0; signal < nsignal; signal++) {
81 sigma_state[2 * signal] = 0.000001;
82 sigma_state[2 * signal + 1] = 0;
89 for (
unsigned int signal = 0; signal < nsignal; signal++) {
90 measure[signal] = 3 + 2 * signal;
103 kalman.
initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
108 kalman.
initFilter(nsignal, sigma_state, sigma_measure, dummy, dt);
112 for (
unsigned int iter = 0; iter <= niter; iter++) {
113 std::cout <<
"-------- iter " << iter <<
" ------------" << std::endl;
114 for (
unsigned int signal = 0; signal < nsignal; signal++) {
115 measure[signal] = 3 + 2 * signal + 0.3 * sin(
vpMath::rad(360. / niter * iter));
117 std::cout <<
"measure : " << measure.t() << std::endl;
123 flog << kalman.
Xest.
t() << std::endl;
125 std::cout <<
"Xest: " << kalman.
Xest.
t() << std::endl;
131 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
This class provides an implementation of some specific linear Kalman filters.
void initFilter(unsigned int nsignal, vpColVector &sigma_state, vpColVector &sigma_measure, double rho, double dt)
void filter(vpColVector &z)
void setStateModel(vpStateModel model)
@ stateConstVel_MeasurePos
@ stateConstVelWithColoredNoise_MeasureVel
static double rad(double deg)