Manifold Factor Graph Optimization-Based GNSS/IMU Integrated Navigation Testing Platform for Urban Vehicles

Multisource sensor fusion has become a research focus in navigation and positioning. However, the Kalman filter algorithm faces issues with real-time performance and positioning accuracy when handling asynchronous data fusion. The development of factor graph algorithms has significantly improved these issues. Meanwhile, research on vehicle-based manifold factor graph optimization algorithms in urban environments is relatively scarce. Therefore, this paper proposes a GNSS/IMU integrated navigation testing platform based on manifold factor graph optimization. The platform is designed to analyze the real-time performance and accuracy of the optimization algorithm under different path ranges. It optimizes IMU pre-integration using Lie group algorithms and constructs a manifold pre-integration factor model with keyframes. The testing algorithm integrates IMU pre-integration factors, GNSS factors, and state prior factors to achieve synchronous multisource fusion. By optimizing a sliding window, it performs nonlinear optimization of variables within the window and outputs the final navigation state. Simulation results show the proposed testing algorithm improves positioning accuracy by 96.67%, 95.36%, and 89.55% compared to three other algorithms. It maintains execution times below 10 seconds, demonstrating high real-time performance and efficiency.