An asynchronous fusion algorithm of the SINS/GPS/CNS based on factor graph

Integrated navigation system can improve the system accuracy utilizing the redundant information. But in general there are asynchronous problems of multi-sensors which will affect the accuracy of the integrated navigation system significantly. And traditional fusion algorithms usually cast lots of computation consumption. So in this paper a new fusion algorithm based on factor graph and incremental inference was proposed to solve the above problems. Taken the SINS/GPS/CNS integrated navigation system as an example, this new nonlinear fusion algorithm can fuse the measurements of asynchronous sensors using the factor graph effectively. The new algorithm based on the factor graph was simulated to verify its effectiveness. The azimuth misalignment error is convergence to -0.18° in 100s with the proposed algorithm while the azimuth misalignment error is convergence to -0.51° in 250s with the traditional fusion method based on the EKF. The simulation results showed that the initial alignment speed and accuracy of the SINS/GPS/CNS are enhanced greatly compared with the traditional method.