fixed error in DVL kalman filter and added sonar navigation