OiSAM-FGO: an efficient factor graph optimization algorithm for GNSS/INS integrated navigation system

Abstract In recent years, the Factor Graph Optimization (FGO) algorithm has gained a great attention in the field of integrated navigation owing to its better positioning performance than the traditional filter-based approaches. However, the practical application of the FGO algorithm remains challenging due to its significant computational complexity and processing time consumption, especially for the case of limited storage and computation resources. In order to overcome the problem, we first conduct a thorough analysis of the factor graph model for the Global Navigation Satellite System/Inertial Navigation System (GNSS/INS) integrated navigation. Then, based on the Incremental Smoothing and Mapping (iSAM), an Optimized iSAM (OiSAM) algorithm is proposed to efficiently solve the optimization problem in FGO, with reducing computational load and required memory resources. For the re-linearization problem, we propose a novel Adaptive Joint Sliding Window Re-linearization (A-JSWR) algorithm combining periodic and on-demand re-linearization to further improve the efficiency of OiSAM. Finally, the OiSAM-FGO method utilizing OiSAM and A-JSWR is presented for the GNSS/INS integrated navigation. The experiments on real-world datasets demonstrated that the OiSAM-FGO can reduce the time consumption of the optimization procedure by up to 52.24%, while achieving a performance equivalent to that of the State-of-the-Art (SOTA) FGO method and superior to the Extended Kalman Filter (EKF) method.