Fault Detection and Interactive Multiple Models Optimization Algorithm Based on Factor Graph Navigation System

Abstract

Accurate and stable positioning is significant for vehicle navigation systems, especially in complex urban environments. However, urban canyons and dynamic interference make vehicle sensors prone to disturbance, leading to vehicle positioning errors and even failures. To address these issues, an adaptive loosely coupled IMU/GNSS/LiDAR integrated navigation system based on factor graph optimization with sensor weight optimization and fault detection is proposed. First, the factor nodes and system framework are constructed based on error models of sensors, and the optimization method principle is derived. Second, the interactive multiple-model algorithm based on factor graph optimization (IMMFGO) is utilized to calculate and adjust sensor weights for global optimization, which will reduce the impact of disturbed sensors. Finally, a multi-stage fault detection, isolation, and recovery (MSFDIR) strategy is implemented based on the IMMFGO results and IMU pre-integration measurements, which can detect significant sensor faults and optimize the system structure. Vehicle experiments show that our IMMFGO method generally obtains better performance in positioning accuracy by 23.7% compared to adaptive factor graph optimization (AFGO) methods, and the MSFDIR strategy possesses the capability of fault sensor detection, which provides an essential reference for multi-source vehicle navigation systems in urban canyons.