30 #ifndef KALMAN_FILTER1_H 31 #define KALMAN_FILTER1_H 41 void set(
const float x0 = 0.0,
42 const float sigma0 = std::numeric_limits<float>::infinity())
48 const float sigma0 = std::numeric_limits<float>::infinity())
52 void predict(
const float x_plus,
const float sigma_plus)
57 void measure(
const float x_in,
const float sigma_in)
59 if (std::isinf(sigma_in))
61 if (std::isinf(sigma_))
70 float kt = sigma_ * sigma_ / (sigma_ * sigma_ + sigma_in * sigma_in);
71 x_ = x_ + kt * (x_in -
x_);
72 sigma_ = (1.0 - kt) * sigma_;
76 #endif // KALMAN_FILTER1_H void measure(const float x_in, const float sigma_in)
KalmanFilter1(const float x0=0.0, const float sigma0=std::numeric_limits< float >::infinity())
void predict(const float x_plus, const float sigma_plus)