An Improved Cooperative Localization Algorithm Based on FG for MAUVs with Communication Delay

To deal with underwater acoustic communication delay in a Cooperative Localization (CL) system for multiple Autonomous Underwater Vehicles (MAUVs), an improved master-slave CL algorithm based on Factor Graph (FG) is proposed in this paper. First, two-dimension kinematic model of AUV and range measurement equation of a CL system for MAUVs are established. Then, the corresponding FG model is constructed, and Sum-Product Algorithm (SPA) is used to derive message passing algorithm among different nodes on the FG model through two iterations, which can obtain estimation of AUV position in the FG model including cycles. Aiming at delayed measurements induced by underwater acoustic communication, the proposed CL algorithm based on FG matches delayed measurements with corresponding states and then update current states. Simulation results illustrate that the localization accuracy of the proposed CL algorithm is effectively improved compared with Extended Kalman Filter (EKF) and FG-based CL algorithm without considering communication delay.