Robust Adaptive Algorithm Based on SE(3) and Its Application in SINS/DVL

Aimed at the problem that SINS/DVL integrated navigation is vulnerable to interference in complex environment, a robust adaptive algorithm based on SE(3) is proposed. By introducing the Lie group/Lie algebra theory and the robust adaptive strategy into the orthogonal transformed cubature Kalman filter (TCKF), the estimated states of TCKF are made to incorporate the Special Euclidean group (SE(3)), and the state space inconsistency problem is improved. The anomaly measurement is adaptively reconstructed based on the innovation vector, by using the chi-square test and the Huber method. The experimental results of SINS/DVL show that the proposed method has a better spatial consistency and robustness than the traditional methods.