Consistent Extended Kalman Filter-Based Cooperative Localization of Multiple Autonomous Underwater Vehicles

In order to solve the problem of inconsistent state estimation when multiple autonomous underwater vehicles (AUVs) are co-located, this paper proposes a method of multi-AUV co-location based on the consistent extended Kalman filter (EKF). Firstly, the dynamic model of cooperative positioning system follower AUV under two leaders alternately transmitting navigation information is established. Secondly, the observability of the standard linearization estimator based on the lead-follower multi-AUV cooperative positioning system is analyzed by comparing the subspace of the observable matrix of state estimation with that of an ideal observable matrix, it can be concluded that the estimation of state by standard EKF is inconsistent. Finally, aiming at the problem of inconsistent state estimation, a consistent EKF multi-AUV cooperative localization algorithm is designed. The algorithm corrects the linearized measurement values in the Jacobian matrix for cooperative positioning, ensuring that the linearized estimator can obtain accurate measurement values. The positioning results of the follower AUV under dead reckoning, standard EKF, and consistent EKF algorithms are simulated, analyzed, and compared with the real trajectory of the following AUV. The simulation results show that the follower AUV with a consistent EKF algorithm can keep synchronization with the leader AUV more stably.