23 #include <utils/kalman/kalman_1d.h> 58 float help = __sig*__sig + __noise_x*__noise_x + __noise_z*__noise_z;
59 __mu = ((__sig*__sig + __noise_x*__noise_x) * observe + __noise_z*__noise_z*__mu) / help;
60 __sig = sqrt( (__sig*__sig + __noise_x*__noise_x)*__noise_z*__noise_z / help );
109 return predict(__mu, vel, steps, noise_z);
122 return mu + steps * (vel + noise_z);
float predict() const
Predicts the next position based on the past observations.
Fawkes library namespace.
~KalmanFilter1D()
Destructor.
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.