A Novel Factor Graph and Cubature Kalman Filter Integrated Algorithm for Single-Transponder-Aided Cooperative Localization

Cooperative localization (CL) of underwater multi-AUVs is vital for numerous underwater operations. Single-transponder-aided cooperative localization (STCL) is regarded as a promising scheme for multi-AUVs CL, benefiting from the fact that an accurate reference is adopted. To improve the positioning accuracy and robustness of STCL, a novel Factor Graph and Cubature Kalman Filter (FGCKF)-integrated algorithm is proposed in this paper. In the proposed FGCKF, historical information can be efficiently used in measurement updating to overcome uncertain observation environments, which greatly helps to improve the performance of filtering progress. Furthermore, Adaptive CKF, sum product, and Maximum Correntropy Criterion (MCC) methods are designed to deal with outliers of acoustic transmission delay, sound velocity, and motion velocity, respectively. Simulations and experiments are conducted, and it is verified that the proposed FGCKF algorithm can improve positioning accuracy and robustness greatly than traditional filtering methods.