Robust Inertial-Aided Underwater Localization Based on Imaging Sonar Keyframes

This article focuses on feature-based underwater localization and navigation for autonomous underwater vehicles (AUVs) using 2-D imaging sonar measurements. The sparsity of underwater acoustic features and the loss of elevation angle in sonar images may introduce wrong feature matches or insufficient features for optimization-based underwater localization (i.e., underconstrained/degeneracy cases). This motivates us to propose a novel inertial-aided sliding window optimization framework to improve the estimation accuracy and the robustness to front-end outliers. Concretely, we first discriminate underconstrained/well-constrained sonar frames and define sonar keyframes (SKFs) based on the Jacobian matrix derived from odometry and sonar measurements. To utilize the past well-constrained SKFs mostly, we design a size-adjustable windowed back-end optimization scheme based on singular values. We also prove that the landmark triangulation failure (navigation problem) caused by sonar motion can be solved in 2-D scenes. Comparative simulation and evaluation on a public dataset show that the proposed method outperforms the existing ones in pose estimation and robustness even without loop closure and also ensures the real-time performance for online applications.