An Invariant Error-Based SINS/DVL/USBL Tightly Coupled Integrated Navigation Algorithm and Its Robust State Estimator

The existing strapdown inertial navigation system (SINS)/Doppler velocity log (DVL)/ultra-short baseline system (USBL) integrated navigation error models directly apply algebraic differences to define the vector error, which is not strictly accurate. Considering that the invariant error definitions derived from Lie group theory have been used in the navigation field to obtain more accurate linear error models, in this paper, a new SINS/DVL/USBL integrated navigation system is investigated based on the invariant error definition and tightly coupled structure. In order to suppress the effect of measurement outliers on navigation accuracy, a new statistical similarity measure (SSM)-based multiple adaptive outlier-robust state estimator (MAORSE) is proposed. It decomposes the noise covariance matrix into multiple sub-matrices. Through the estimation and adjustment of sub-matrices, the influence of abnormal observations on the filtering results is weakened while realizing the full utilization of normal observation information. The test results demonstrate the effectiveness and superiority of the proposed invariant error-based SINS/DVL/USBL tightly coupled integrated navigation algorithm and SSM-based MAORSE.