Keyframe-Based Imaging Sonar Localization and Navigation using Elastic Windowed Optimization

This paper reports a sonar keyframe-based elastic windowed optimization scheme for underwater localization and navigation using a 2D forward-looking imaging sonar. Due to the sparse acoustic features and the missing elevation angle in sonar images, degeneracy cases (namely under-constrained), such as relative pose ambiguity, exist in underwater sonar simultaneous localization and mapping (SLAM). To extract and fully utilize informative constraints from past sonar frames, we first identify the degeneracy case and choose sonar keyframes (KFs) using the effective information volume criteria. Secondly, we present an elastic windowed optimization framework in which the window size is adjusted according to certain rules. This scheme is also more robust to outliers from the frontend. Finally, the feasibility and effectiveness of the proposed method are verified through the comparative simulation.