23#include <utils/kalman/kalman_1d.h>
59 float help = sig_ * sig_ + noise_x_ * noise_x_ + noise_z_ * noise_z_;
60 mu_ = ((sig_ * sig_ + noise_x_ * noise_x_) * observe + noise_z_ * noise_z_ * mu_) / help;
61 sig_ = sqrt((sig_ * sig_ + noise_x_ * noise_x_) * noise_z_ * noise_z_ / help);
107 return predict(mu_, vel, steps, noise_z);
120 return mu + steps * (vel + noise_z);
float predict() const
Predicts the next position based on the past observations.
void filter(float observe)
Filters an observation.
KalmanFilter1D(float noise_x=1.0, float noise_z=1.0, float mu=0.0, float sig=1.0)
Constructor.
~KalmanFilter1D()
Destructor.
Fawkes library namespace.