Fix kalman filter reset procedure