A novel terrain-aided tightly coupled navigation system based on invariant federated filter for underwater vehicles

Abstract

Terrain-aided navigation (TAN) provides absolute positioning for underwater vehicles during long-term operations. However, conventional filter-based TAN systems typically do not explicitly model inertial measurement unit (IMU) biases as part of the state. To address the issue, this paper proposes a novel terrain-aided tightly coupled system based on invariant federated filter to attenuate the impact of IMU biases on navigation accuracy. Unlike conventional TAN architectures composed of three relatively independent modules, the proposed system directly fuses raw sensor measurements within a unified invariant federated filtering framework. In the filter, IMU biases and navigation errors are jointly augmented into the state vector. The corresponding state-space model is derived based on the Lie-group strapdown inertial navigation system error model. By explicitly separating IMU biases from navigation states, the proposed model improves estimation accuracy. Simulation results demonstrate that the proposed system provides accurate and robust positioning results for different initial misalignment angles, different initial position errors, and IMUs of different grades.