Fix Kalman filter usage