Outlier Mitigation for Cooperative Positioning Using Graduated Nonconvexity Truncated Least-Squares Cost

In urban environments, cooperative positioning (CP) based on Global Navigation Satellite Systems (GNSS), inter-vehicle communication and integrated sensing and communication (ISAC) experiences non-line-of-sight (NLOS) receptions and multipath delays, limiting positioning reliability for safety-related applications. This paper proposes a factor graph optimization (FGO) method that integrates GNSS pseudorange, Doppler, and ultra-wideband (UWB) ranging in a distributed framework. The truncated least squares (TLS) cost is adopted to mitigate outliers, and a graduated non-convexity (GNC) strategy is further employed to address the local optimum issue arising from the non-convexity of the TLS cost. The experimental results demonstrate that, compared with traditional fault detection and exclusion (FDE) based CP method, the proposed approach reduces the mean horizontal positioning error from 1.76 m to 0.89 m.