INS/GNSS/UWB/OD Robust Navigation Algorithm Based on Factor Graph

Multi-source navigation system can complete cross-scene and high-precision navigation tasks, and its robustness is a research focus in this field. In this paper, a robust INS/GNSS/UWB/OD navigation algorithm based on factor graph is proposed for unmanned ground vehicle (UGV). The sum-product algorithm is used to complete multi-sensor information fusion. In order to ensure that the algorithm can still have strong robustness in various complex environments, this paper designs a regulation factor by using residual error and its covariance, adaptively adjusts the measurement covariance matrix, so that the navigation system can still achieve high precision navigation when the accuracy of the sensor is reduced due to occlusion or faults. In order to evaluate the robust navigation algorithm proposed in this paper, on-vehicle experiments were carried out under different conditions. The results show that the proposed algorithm significantly improves the robustness and accuracy of the multi-source navigation system.