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

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.

💡 Research Summary

This paper addresses the problem of cooperative localization for multi‑vehicle systems (MVS) operating in GNSS‑denied environments, with a focus on three‑dimensional (3‑D) scenarios where vehicle states include position, velocity, attitude (quaternion), and inertial measurement unit (IMU) biases. In distributed cooperative localization (DCL), each vehicle estimates its own state and exchanges only limited information with neighbors, avoiding the communication overhead and single‑point‑of‑failure issues of centralized cooperative localization (CCL). However, DCL cannot track the unknown cross‑correlations that arise when vehicles use each other’s estimates, which can lead to inconsistent updates if not handled properly.

Covariance Intersection (CI) is a well‑known technique for fusing estimates under unknown correlations. Traditional CI implementations minimize a scalar cost such as the trace or determinant of the fused covariance matrix. While effective for two‑dimensional (2‑D) unicycle models, these criteria become problematic in 3‑D settings because the state vector expands to 16 dimensions (3‑D position, 3‑D velocity, 4‑D quaternion, 3‑D gyroscope bias, 3‑D accelerometer bias). The large disparity in scale and observability among these components causes the CI cost to be dominated by the position terms; consequently, the covariance of low‑scale, poorly observable states (attitude, biases) inflates dramatically, degrading overall localization performance and even causing divergence over time.

To overcome this limitation, the authors propose Weighted Covariance Intersection (WCI), a two‑fold enhancement of the classic CI framework:

1. Weighting Matrix Derived from INS Error Propagation – By analyzing the error‑state dynamics of an INS, the authors extract sensitivity coefficients that quantify how errors in each state component affect the overall navigation error. These coefficients are assembled into a diagonal weighting matrix W that reflects the relative importance and scale of each state dimension. The fusion cost is then defined as a weighted mean‑squared error (WMSE):  tr(W·Σ_fused). This shifts the optimization focus from raw covariance magnitude to a task‑oriented metric that balances position, velocity, attitude, and bias uncertainties.

2. Concurrent Fusion of Multiple Range Measurements – In realistic swarms, a vehicle simultaneously receives several distance measurements to neighboring vehicles and to fixed anchors. Instead of processing them sequentially (which would duplicate information and increase computational load), the authors formulate a single fusion step that combines all measurements. A weight vector ω (subject to ∑ω=1, ω≥0) is introduced to blend the individual information sources. The optimal ω is obtained by minimizing the WMSE under the same weighting matrix W, leading to a quadratic programming problem that can be solved in real time.

Mathematically, given two Gaussian estimates (μ₁, Σ₁) and (μ₂, Σ₂), the WCI fused covariance and mean are:

Σ_WCI⁻¹ = ω·Σ₁⁻¹ + (1‑ω)·Σ₂⁻¹
μ_WCI = Σ_WCI·