Bundle Adjustment-Based Sonar-Inertial Odometry for Underwater Navigation

A sonar-inertial odometry (SIO) algorithm based on bundle adjustment is proposed in this study for underwater navigation. To improve the precision of vehicle state estimation, SIO fuses the measurements of two sensors that are tightly cou-pled, that is the acoustic observations from a forward-looking sonar (FLS) and the inertial measurements from an inertial measurement unit (IMU). In particular, SIO can establish the relative pose constraints of multi-frame sonar measurements by employing bundle adjustment without the prior assumption about the environment. The initial pose used to estimate the sonar inter-frame motion is dead reckoning with inertial measurements. Combined with factor graph optimization, the two factors are elegantly integrated into the SIO framework: (1) Inertial measurement factor with preintegration, is calculated for the relative constraints between sonar frames only, thereby reducing the computing resources consumption. (2) Sonar measurement factor using reprojection errors, is created by investigating the relationship between the sonar imaging and the motion transformation, which corrects for drift in dead reckoning via feature perception. Furthermore, the analytical form of the sonar measurement factor is completely developed, and the simulation shows that the proposed SIO outperforms dead reckoning in terms of accuracy.