Weighted Covariance Intersection for Range-based Distributed Cooperative Localization of Multi-Vehicle Systems

Abstract

Cooperative localization enables autonomous navigation for multi-vehicle systems (MVS) in GNSS-denied environments. Among the available architectures, distributed cooperative localization (DCL) is attractive for its robustness and scalability in large-scale MVS. To address the challenge of untrackable state correlations between vehicles in a distributed framework, covariance intersection (CI) has been introduced as a means to fuse inter-vehicle measurements. However, existing studies typically treat CI as a plug-in technique, directly applying traditional optimization criteria and focusing mainly on simple two-dimensional (2D) scenarios. When extended to three-dimensional (3D) cooperative localization with higher-dimensional states and pronounced disparities in scale and observability among state components, traditional CI criteria fail to maintain balanced estimation performance across the full state, and some state components suffer substantial accuracy degradation. This limitation calls for task-oriented improvements to the CI fusion process. In this paper, we introduce a weighting mechanism, called weighted covariance intersection (WCI), to regulate the CI fusion process in 3D DCL. We further develop a concurrent fusion strategy for multiple distance measurements and design a dedicated weighting matrix based on inertial navigation system (INS) error propagation. The method supports diverse MVS platforms, including unmanned aerial vehicle (UAV) swarms and autonomous ground vehicle (AGV) fleets, in complex 3D environments. Simulation results show that the proposed WCI significantly improves cooperative localization performance over traditional CI, while the distributed framework offers clear advantages over the centralized counterpart in robustness and scalability.