DVL KalmanFilter fix by Alex