Kalman filter implementation is broken since build #109