  This article focuses on feature-based underwater localization and navigation
for autonomous underwater vehicles (AUVs) using 2D 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. under-constrained/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 under-constrained/
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 2D scenes. Comparative simulation and evaluation on a public
dataset show 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.
