Flexible and Consistent Cooperative Navigation for Unmanned Systems Using Tightly-coupled GNSS/IMU/UWB Integration

Cooperative navigation (CN) is a promising solution for accurate and reliable positioning in multi-agent systems, including unmanned aerial, ground, and underwater vehicles. Relative ranging via ultra-wide band (UWB) is a typical method for achieving cooperation. However, the lack of intuitive tools to describe measurement topologies and cooperative architectures limits the understanding of their influence on CN performance. Additionally, correlations between relative ranging measurements must be considered during distributed fusion, which is often unknown to the estimators. This paper introduces a flexible and consistent framework that leverages Split Covariance Intersection (SCI) to obtain optimal consistent estimates in distributed multi-agent systems. Firstly, we integrated graph theory to represent measurement topology and cooperative architecture, providing a structured model of agent interactions and information flow. This integration enhances the framework's adaptability and flexibility, making it applicable to both centralized and distributed architectures. Secondly, we developed a tightly coupled model that fuses global navigation satellite system (GNSS), inertial measurement unit (IMU), and UWB sensor data. State estimation is performed using a Sequential Kalman Filter, which significantly improves the accuracy and robustness of the estimation process. Lastly, we employed an SCI-based method to efficiently handle inter-filter correlations. This approach reduces the computational complexity of covariance matrix derivation while maintaining estimation precision. Both simulations and field experiments validate the algorithm's positioning performance, demonstrating its ability to propagate high-precision sensor advantages across clusters and resolve inconsistencies in state estimation. This advancement greatly improves the robustness and scalability of cooperative navigation systems, providing a promising solution for complex multi-agent tasks in large-scale environments.