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);