WHF-VIO: Wave-Aware Height Factor for Robust Visual–Inertial Odometry of Unmanned Surface Vehicles

Abstract

In global navigation satellite system (GNSS)-denied conditions, long-duration state estimation of unmanned surface vehicles (USVs) is prone to drift accumulation, especially in the vertical direction where observability is weak over near-planar motions. To improve localization accuracy and robustness in water-surface environments, this article proposes a wave-aware height factor (WHF) and tightly integrates it into factor graph optimization (FGO). The WHF constructs a physically consistent height residual based on a surface–vehicle kinematic coupling model, explicitly compensating the apparent vertical variation induced by attitude–heave coupling, so that reasonable wave-excited attitude oscillations are not mistaken for height drift. Moreover, an adaptive covariance scheme is designed to dynamically adjust the constraint strength of WHF during strong wave disturbances or unstable observations, thereby maintaining backend consistency and suppressing error accumulation. Extensive real-world lake experiments across multiple operating conditions demonstrate that the proposed method effectively mitigates height drift and consistently improves planar positioning accuracy and robustness.