A Factor Graph Optimization Method for High-Precision IMU-Based Navigation System

Multi-sensor integrated navigation systems based on factor graph are increasingly used on indoor robots, UAVs, and other vehicles. The output information of the equipped low-cost inertial measurement unit (IMU) is usually processed by IMU pre-integration techniques. As the accuracy of IMU increases, the traditional factor graph using the IMU pre-integration method need to be improved. This paper proposes a factor graph optimization algorithm for high-precision IMU based navigation system. An improved IMU pre-integration method is used in the algorithm to deal with the data from inertial sensors. Different from traditional methods, the effect of the curvature of the Earth’s surface on the IMU pre-integration method is taken into account. Meanwhile, the parameters affecting the accuracy of the IMU pre-integration method are corrected by the estimated navigation state of the carrier. Thus, a more accurate relative constraint is constructed. After that, this constraint and other measurement information are fused by the factor graph optimization algorithm. Finally, different simulation tests and field vehicle tests are carried out to validate the performance of the proposed method. The test results show that the proposed method can improve the carrier positioning accuracy by 20% to 90% when using high-precision inertial sensors under different conditions.