Improved Three-Dimensional Terrain-Aided AUV Navigation Using Error State Filtering

Kalman filtering has been commonly used in terrain-aided navigation (TAN) applications for autonomous underwater vehicles (AUVs) where digital terrain maps are available and bottom terrains are sufficiently varied. Current Kalman filtering based TAN literature notably lacks an explicit consideration of navigational sensor bias errors which can have impacts on positioning accuracy and payload sensor data such as sonars. This paper builds on our previous work on 2D TAN by investigating the performance of a low-cost AUV navigating underwater in 3D and estimating navigational sensor bias errors using existing digital terrain maps using an error-state, extended Kalman filter (ES-EKF). Preliminary results based on Monte-Carlo runs suggest that ES-EKF can effectively estimate the vehicle position (Initial error < 132 m), velocity, orientation, most sensor biases, and small ocean currents (< 0.5 m/s).