Kalman filter bug fixed