30 #ifndef TRACK_ODOMETRY_KALMAN_FILTER1_H 31 #define TRACK_ODOMETRY_KALMAN_FILTER1_H 43 void set(
const double x0 = 0.0,
44 const double sigma0 = std::numeric_limits<double>::infinity())
50 const double sigma0 = std::numeric_limits<double>::infinity())
54 void predict(
const double x_plus,
const double sigma_plus)
59 void measure(
const double x_in,
const double sigma_in)
61 if (std::isinf(sigma_in))
63 if (std::isinf(sigma_))
72 double kt = sigma_ * sigma_ / (sigma_ * sigma_ + sigma_in * sigma_in);
73 x_ = x_ + kt * (x_in -
x_);
74 sigma_ = (1.0 - kt) * sigma_;
79 #endif // TRACK_ODOMETRY_KALMAN_FILTER1_H
void measure(const double x_in, const double sigma_in)
KalmanFilter1(const double x0=0.0, const double sigma0=std::numeric_limits< double >::infinity())
void predict(const double x_plus, const double sigma_plus)