RA-SIDO: Robust and Adaptive Sonar–Inertial–Depth Odometry for Consistent Underwater Acoustic 3D Mapping

Autonomous acoustic remote sensing of underwater infrastructure is challenging due to the physical characteristics of 3D sonar and the geometric degeneracy commonly encountered in feature-poor underwater environments. Accurate localization is essential for integrating sequential sonar observations into globally consistent 3D maps; however, existing odometry methods often rely on isotropic noise assumptions despite the highly directional nature of acoustic sensing. This mismatch may cause unreliable measurements to be over-trusted, leading to severe trajectory drift and distortion in sonar-derived 3D reconstructions. To address these challenges, we propose RA-SIDO, a robust and adaptive tightly coupled 3D sonar–inertial–depth odometry framework based on the Error-State Iterated Kalman Filter (ESIKF), which fuses measurements from a 3D sonar, an inertial measurement unit (IMU), and a depth sensor for reliable underwater acoustic mapping. The proposed method introduces two mechanisms to handle sonar-specific uncertainties: (1) a physics-based anisotropic acoustic measurement model that distinguishes high-resolution radial range measurements from highly uncertain cross-range angular measurements; (2) an online degeneracy-awareness module that continuously evaluates the minimum eigenvalue of the translational information matrix and dynamically adjusts sensor fusion weights to avoid over-trusting ill-conditioned constraints. Real-world experiments were conducted with an unmanned surface vehicle in underwater infrastructure inspection scenarios. RA-SIDO achieved an ATE RMSE of 0.8924m, reducing the error by 16.8% compared with SIDO, the strongest baseline. In addition, the proposed method effectively suppresses longitudinal slip and produces globally consistent 3D acoustic maps of submerged structures. These results validate the potential of RA-SIDO as a robust localization and mapping solution for underwater remote sensing, infrastructure inspection, and acoustic 3D reconstruction in challenging aquatic environments.