High-precision initialization and acceleration of particle filter convergence to improve the accuracy and stability of terrain aided navigation

Abstract

Initial positioning errors and the low adaptability of a priori digital elevation maps result in large positioning uncertainty intervals in the initial stage of terrain-aided navigation (TAN). This produces pseudo-peaks and mismatches in the initial position likelihood function and renders the convergence of the particle filter (PF) slow and unstable, while even causing divergence. Thus, the occurrence of the "kidnapped robot problem" is highly probable during the initial stage of TAN and is a scenario frequently faced by deep-sea and ultra-long-range underwater vehicles. In this study, a PF initialization method based on non-linear multi-terrain aided fusion position (NLMTP) is proposed to improve the stability and accuracy of TAN. NLMTP uses the terrain-aided position (TAP) information during the initial stage of TAN to estimate the high-precision probability distribution of the starting position via backward smoothing. Accordingly, a PF initialization method for non-Gaussian prior distribution probability is proposed to improve the convergence speed of the PF during the initial stage of underwater TAN. Finally, a performance comparison of PF initialized via the NLMTP, TAP confidence interval, and TERCOM methods was performed using the survey data obtained via onboard sensors. The experimental results show that NLMTP initialization improves the convergence speed and positioning accuracy of PF in the initial TAN phase; this improvement is clear in the low terrain-adaptability area.

Keywords: Autonomous underwater vehicle; Deep-sea navigation; Nonlinear fusion; Particle filter; Terrain-aided navigation.