Marginalized Point Mass Filter with Estimating Tidal Depth Bias for Underwater Terrain-Aided Navigation

Terrain-aided navigation is a promising approach to submerged position updates for autonomous underwater vehicles by matching terrain measurements against an underwater reference map. With an accurate prediction of tidal depth bias, a two-dimensional point mass filter, only estimating the horizontal position, has been proven to be effective for terrain-aided navigation. However, the tidal depth bias is unpredictable or predicts in many cases, which will result in the rapid performance degradation if a two-dimensional point mass filter is still used. To address this, a marginalized point mass filter in three dimensions is presented to concurrently estimate and compensate the tidal depth bias in this paper. In the method, the tidal depth bias is extended as a state variable and estimated using the Kalman filter, whereas the horizontal position state is still estimated by the original two-dimensional point mass filter. With the multibeam sonar, simulation experiments in a real underwater digital map demonstrate that the proposed method is able to accurately estimate the tidal depth bias and to obtain the robust navigation solution in suitable terrain.