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

TL;DR

This paper introduces a weighted covariance intersection method to improve distributed cooperative localization accuracy for multi-vehicle systems in 3D environments, addressing limitations of traditional CI methods.

Contribution

The paper proposes a novel weighted covariance intersection technique with a new fusion strategy and weighting matrix, enhancing 3D multi-vehicle localization accuracy.

Findings

WCI outperforms traditional CI in simulation tests.

Distributed approach offers better robustness and scalability than centralized methods.

Significant improvement in state estimation accuracy for complex 3D scenarios.

Abstract

Cooperative localization is considered a key solution for enabling autonomous navigation of multi-vehicle systems (MVS) in GNSS-denied environments. Among all solutions, distributed cooperative localization (DCL) has garnered widespread attention due to its robustness and scalability, making it well-suited for 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 relative measurements under unknown correlations. However, existing studies treat CI merely as a plug-in method, applying traditional optimization criteria directly and focusing only on simple two-dimensional (2D) scenarios. When directly extended to three-dimensional (3D) scenarios with more complex state space (higher dimensions, additional state components, and significant disparities in scale and observability among state components), traditional methods fail to achieve balanced state estimation across all state components, leading to a significant degradation in the estimation accuracy of some state components. This highlights the need to design specialized mechanisms to improve the data fusion process. In this paper, we introduce a weighting mechanism, namely the weighted covariance intersection (WCI), to regulate the fusion process of CI. A concurrent fusion strategy for multiple distance measurements and a dedicated weighting matrix based on the error propagation rule of the inertial navigation system (INS) are developed for the data fusion process in DCL. Simulation results demonstrate that the proposed WCI significantly enhances cooperative localization performance compared to traditional CI, while the distributed approach outperforms the centralized approach in terms of robustness and scalability.