An Invariant Filtering Method Based on Frame Transformed for Underwater INS/DVL/PS Navigation

Abstract

Underwater vehicles heavily depend on the integration of inertial navigation with Doppler Velocity Log (DVL) for fusion-based localization. Given the constraints imposed by sensor costs, ensuring the optimization ability and robustness of fusion algorithms is of paramount importance. While filtering-based techniques such as Extended Kalman Filter (EKF) offer mature solutions to nonlinear problems, their reliance on linearization approximation may compromise final accuracy. Recently, Invariant EKF (IEKF) methods based on the concept of smooth manifolds have emerged to address this limitation. However, the optimization by matrix Lie groups must satisfy the “group affine” property to ensure state independence, which constrains the applicability of IEKF to high-precision positioning of underwater multi-sensor fusion. In this study, an alternative state-independent underwater fusion invariant filtering approach based on a two-frame group utilizing DVL, Inertial Measurement Unit (IMU), and Earth-Centered Earth-Fixed (ECEF) configuration is proposed. This methodology circumvents the necessity for group affine in the presence of biases. We account for inertial biases and DVL pole-arm effects, achieving convergence in an imperfect IEKF by either fixed observation or body observation information. Through simulations and real datasets that are time-synchronized, we demonstrate the effectiveness and robustness of the proposed algorithm.