Information fusion algorithm of ship navigation system based on factor graph

Abstract

Aiming at the multi-source navigation information fusion problems of marine strapdown inertial navigation system (SINS), Beidou (BD), Doppler velocity log (DVL) and celestial navigation (CNS) with different information update frequency and dynamic change of information availability with environment, an information fusion and fault tolerant algorithm for marine navigation system based on incremental smoothing factor graph is proposed. Firstly, the factorial node models of SINS, BD, DVL and CNS are established, and the fusion architecture is constructed by using the measurement information of all time and the maximum posterior probability estimation method. Then, after inserting new measurement factor nodes, some navigation state variable nodes affected by new measurement factors are identified and updated, and the maximum posteriori estimates of navigation state are calculated by factor graph increment smoothing algorithm. Finally, an information fault tolerant algorithm based on chi-square test is designed for fault detection. Numerical simulation and semi-physical vehicle test show that the proposed algorithm has plug-and-play characteristics and can realize effective fusion of navigation devices with different information update frequencies, and the fusion accuracy is equivalent to that of federated filter. The fault-tolerant algorithm can effectively identify and isolate the fault sensor when the navigation sensor fails.