Robust Underwater Vehicle Pose Estimation via Convex Optimization Using Range-Only Remote Sensing Data

Accurate localization plays a critical role in enabling underwater vehicle autonomy. In this work, we develop a robust infrastructure-based localization framework that estimates the position and orientation of underwater vehicles using only range measurements from long baseline (LBL) acoustic beacons to multiple on-board receivers. The proposed framework integrates three key components, each formulated as a convex optimization problem. First, we introduce a robust calibration function that unifies multiple sources of measurement error—such as range-dependent degradation, variable sound speed, and latency—by modeling them through a monotonic function. This function bounds the true distance and defines a convex feasible set for each receiver location. Next, we estimate the receiver positions as the center of this feasible region, using two notions of centrality: the Chebyshev center and the maximum volume inscribed ellipsoid (MVE), both formulated as convex programs. Finally, we recover the vehicle’s full 6-DOF pose by enforcing rigid-body constraints on the estimated receiver positions. To do this, we leverage the known geometric configuration of the receivers in the vehicle and solve the Orthogonal Procrustes Problem to compute the rotation matrix that best aligns the estimated and known configurations, thereby correcting the position estimates and determining the vehicle orientation. We evaluate the proposed method through both numerical simulations and field experiments. To further enhance robustness under real-world conditions, we model beacon-location uncertainty—due to mooring slack and water currents—as bounded spherical regions around nominal beacon positions. We then mitigate the uncertainty by integrating the modified range constraints into the MVE position estimation formulation, ensuring reliable localization even under infrastructure drift.