This paper introduces the Compartmentalized Covariance Intersection (CCI) algorithm, a consistent technique to fuse measurements in cooperative navigation networks. The algorithm reduces the excess conservatism of standard Covariance Intersection (CI) by assuming that correlation is only present within each measurement stream and not across the different sources. This assumption allows the sources to be compartmentalized and fused with the Kalman equations rather than the CI method, resulting in tighter convergence. The CCI algorithm is applied to a cooperative localization application and is demonstrated to substantially outperform CI, with a covariance that approaches the performance of an ideal centralized estimator in simulations of linear, Gaussian systems. This approach is also demonstrated to be consistent using Monte Carlo simulations. Finally, CCI was used in a laboratory demonstration to perform distributed localization with real-world range measurements between a team of six agents.

This paper presents a novel filter architecture that allows a team of vehicles to collaboratively localize using Terrain Relative Navigation (TRN). The work explores several causes of measurement correlation that preclude the use of traditional estimators, and proposes an estimator structure that eliminates one source of measurement correlation while properly incorporating others through the use of Covariance Intersection. The result is a consistent estimator that is able to augment proven TRN techniques with multi-robot information, significantly improving localization for vehicles in uninformative terrain. The approach is demonstrated using field data from an Autonomous Underwater Vehicle (AUV) navigating with TRN in Monterey Bay and simulated inter-vehicle range measurements. In addition, a Monte Carlo simulation was used to quantify the algorithm's performance on one example mission. Monte Carlo results show that a vehicle operating in uninformative terrain has 62% lower localization error when fusing range measurements to two converged AUVs than it would using standard TRN.

This paper presents the Push-Swap-Wait (PSW) algorithm, a scalable, decentralized and complete approach for multi-robot motion planning in confined spaces. The algorithm builds upon a “push and swap” paradigm that has been used effectively in centralized navigation. This push and swap approach was expanded to apply to decentralized planning by adding a waiting mode to handle situations in which communication between robots is lost. The completeness of the PSW algorithm can be guaranteed in cases where the environment can be modeled as a tree T for which the number of leaf nodes is greater than the number of robots navigating through it. The algorithm has a time complexity that is linear with the number of robots currently within communication, indicating that this algorithm is well suited for scaling to large systems of robots. To validate the PSW algorithm it was implemented successfully in multi-robot simulations and on hardware with four Dr. Robot Jaguar Lite Robots.