A Robust Factor Graph Optimization Algorithm with Dynamic Observation Weight Adjusting

Objectives: The integration of global navigation satellite systems (GNSS) and inertial navigation systems (INS) provides continuous positioning services. However, in complex urban environments, GNSS signals are highly susceptible to multipath effects and non-line-of-sight interference, which can severely degrade the positioning accuracy of integrated navigation systems. Traditional factor graph optimization methods with fixed weights struggle to accommodate the dynamic variability of GNSS measurement noise covariance, while robust algorithms employing the Huber kernel function incur significant computational overhead. Methods: A dynamic weight function-based robust factor graph optimization algorithm (FGO) is proposed, which dynamically adjusts the residual weight coefficients of GNSS observations in the objective function based on post-fit residuals within a sliding window. The algorithm preserves the quadratic form of the objective function by avoiding nonlinear transformations of residual values, ensuring computational efficiency and adaptability to varying observation quality. To process open-source data with the proposed robust algorithm, sliding windows of 10, 30, and 50 seconds were utilized. And analysis robust performance of the proposed algorithm in different size sliding windows. Results: Compared to the inclusion of the Huber kernel function, the inclusion of the dynamic weight function adjustment mechanism improves the average positioning error standard deviation and root mean square by 26% and 12% respectively When the sliding window is set to 30s, a satisfactory equilibrium between robustness and computation time can be attained. Conclusions: Experimental results show that: 1) compared to FGO with the addition of Huber kernel function, the dynamic weight function adjustment can effectively reduce the influence of GNSS outliers, and has positioning higher accuracy and robustness. 2) The proposed robust algorithm does not change the trend of the sliding window size on localization and time consumption.