Outlier-Resistant Cooperative Positioning Method Using Robust Factor Graph Optimization

Cooperative positioning (CP) is able to improve the vehicular positioning performance by introducing the data of multiple vehicles into the position estimation. However, CP methods are vulnerable to measurement outliers in dense urban areas. The existing outlier-resistant CP methods are easy to trap in local optimum and may wrongly reject the outliers when the ratio of outliers to inliers is relatively high. To deal with this problem, a factor graph optimization (FGO) based CP method using Graduated Non-Convexity (GNC) Welsch cost is proposed in this paper. The state-of-art FGO algorithm is used to integrate multi-node and multi-epoch measurements including the Global Navigation Satellite Systems (GNSS) pseudoranges, inter-epoch baselines estimated by GNSS time-differenced carrier phase (TDCP), and inter-vehicle ranging measurements in a centralized framework. The least-square cost in traditional FGO is replaced with the GNC-based Welsch cost so as to enhance the robustness of the proposed method to any kind of outliers in our CP system. The use of GNC can reduce the risk of local optimum by gradually increasing the non-convexity of the Welsch cost. The proposed method can de-weight the outliers correctly even if a large number of outliers exist. The experimental results show the superiority of the proposed method over the existing CP methods in resisting multiple outliers.