An Acoustic-Inertial Pose Estimation Method with Robust Feature Match and Graph Optimization

High-accuracy pose estimate is necessary for underwater vehicle to perform tasks. In this paper, we present a novel pose-graph navigation system for underwater vehicle using forward-looking sonar (FLS) and inertial measurement unit (IMU). The proposed system first uses A-Kaze features to build an acoustic odometry by triangulation and then utilizes the acoustic structure from motion (AFSM) to obtain pose estimates. Next, all pose estimates are added to a pose graph, associated with inertial odometry measurements, to optimize the pose of the underwater vehicle. The effectiveness of the proposed method is verified via simulations in gazebo.