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