Kalman filter bug fix.