Delayed Measurements Optimization Algorithms in Cooperative Positioning Based on GNSS/INS/Relative Distances Fusion Factor Graph Model

The cooperative positioning system based on multi-source fusion is confronted with challenges in fusing delayed measurements among users. To address this issue, a delay measurement optimization algorithm based on the minimum Mahalanobis distance (MMD) selection mechanism within a factor graph framework is first proposed. The time offset between delayed relative distance measurements and navigation states is accounted for by this algorithm, ensuring the selection of the state with the minimum Mahalanobis distance within each delay window. Second, an insertion delayed factor (IDF) method is introduced to estimate time-varying delays. Delay factor nodes are constructed for each navigation state in the delay window, and the time-varying delay is jointly optimized with the delay factor nodes. Experiments show that the IDF improved the accuracy of the MMD and delay-ignored method (DIM) by 20.6% and 41.3%, respectively, in a dynamic semi-open scenario. Additionally, the balancing of the sampling period is taken into account as an effective strategy to significantly reduce the running time.