Update Kalman filter routine