An Acoustic Ranging Measurement Aided SINS/DVL Integrated Navigation Algorithm Based on Multivehicle Cooperative Correction
Bo Xu, Junmiao Hu, Yu Guo
Abstract
In order to improve the positioning accuracy of two leader autonomous underwater vehicles (AUVs) in underwater cooperative navigation formation, a decentralized cooperative localization algorithm based on adaptive cubature Kalman filter (ACKF-DCL) for strapdown inertial navigation system (SINS)/Doppler velocity log (DVL) integrated navigation system is proposed. Firstly, the kinematic equations of the SINS are used to establish the direct filtering approach model for integrated navigation. Secondly, the Sage-Husa adaptive filtering algorithm and the sequential Kalman filtering (SKF) method are introduced into the nonlinear cubature Kalman filter (CKF) to adjust adaptively the measurement noise and maintain the positivity of the covariance matrix. Finally, the cross-correlation covariance between two AUVs is derived according to the characteristics of a decentralized data structure, so as to track the inter-AUV dependencies. And the acoustic ranging measurement is introduced to achieve relative measurement update. The lake trial demonstrates the proposed algorithm’s effectiveness and superiority, and the results show that the algorithm can effectively track the measurement noise covariance matrix and improve the positioning accuracy of both AUVs by position correction.