Master–slave AUV cooperative navigation algorithm based on nonlinear error compensation of observation matrix

Abstract The master–slave autonomous underwater vehicle (AUV) cooperative navigation (CN) system employs the extended Kalman filter (EKF) to fuse the low-precision estimated position of the slave AUV with the high-precision master–slave ranging data, thereby correcting the positioning error of the slave AUV and enhancing its navigation accuracy. However, the EKF-based CN algorithm shows significant positioning errors when the observation model suffers from substantial linearization errors. Therefore, this paper proposes a master–slave AUV CN algorithm based on observation matrix compensation to reduce the impact of the linearization error of the observation model on positioning accuracy and improve the slave-AUV positioning accuracy. Based on the EKF algorithm framework, this method calculates the partial derivative of the linearization error of the ranging equation to the position state vector of the slave-AUV to obtain the correction component of the observation matrix, compensating for the linearization error and effectively enhancing the positioning accuracy of the slave-AUV. The accuracy and applicability of the proposed method were rigorously verified using simulation and lake water field trials. Compared to the EKF CN algorithm, the positioning error of the proposed algorithm is significantly smaller.