Improved Factor Graph Algorithm for Adaptive Weight Function Based on Residual Adjustment

To address the issue of traditional factor graph methods being unable to handle the dynamic change in sensor measurement accuracy during the operational process, an adaptive weight function is introduced and an improved factor graph method based on adaptive weight is proposed. By calculating the residual between the predicted value of inertial preintegration and the measured value of auxiliary sensors in real-time, the fusion information weight of the corresponding factor nodes are dynamically adjusted. Compared with traditional factor graph algorithms, this method can improve the optimization accuracy and robustness of factor graph algorithms in the situation of step faults, gradual faults, or rejection faults in auxiliary sensors. The simulation experimental results show that when the auxiliary sensor produces measurement faults, compared with traditional factor graph method, the improved factor graph method based on adaptive weights has higher robustness and accuracy. When measurement faults occur in auxiliary sensors, its position, velocity, and attitude estimation accuracy RMSE values have been improved by more than 45%.