Uncertainty-aware manifold and factor graph fusion for cooperative INS/DVL/LBL localization

Abstract Accurate and robust underwater localization remains challenging due to limited absolute positioning availability, asynchronous multi-sensor updates, and non-Gaussian measurement disturbances. This paper presents MEKF-SWFGO, an uncertainty-aware hybrid framework that combines a Manifold Extended Kalman Filter (MEKF) front-end with a Sliding-Window Factor Graph Optimizer (SWFGO) back-end for cooperative INS/DVL/LBL navigation. The front-end enforces attitude consistency on the Lie group SO(3), employs a trapezoidal IMU preintegration for stable propagation, and applies a Huber M-estimator to downweight outliers. The back-end jointly refines recent trajectory segments using numerical Jacobian between-factors and adaptive covariance weighting, while an uncertainty-driven switching mechanism balances real-time filtering and delayed global refinement. Extensive experiments on real AUV data demonstrate that MEKF-SWFGO substantially improves localization accuracy and robustness: total position RMSE is reduced from 33.67 m (CEKF), 21.46 m (MEKF) and 17.05 m (UKF) to 12.10 m.Velocity RMSE falls to 0.44 m/s, representing an 19.1% reduction relative to UKF. Yaw RMSE is improved to 0.74°, a 50.4% reduction over CEKF. Statistical metrics and CDF analyses confirm that MEKF-SWFGO yields tighter, less heavy-tailed error distributions in position, velocity and attitude. These results indicate that the proposed uncertainty-aware manifold and sliding-window fusion is a practical and effective solution for resilient underwater navigation in real operating conditions.