Cooperative relative localization using range measurements without a priori information

The ability for autonomous vehicles to cooperatively navigate, especially in GPS denied environments, is becoming increasingly important. It also requires the ability to initialize, or reinitialize estimation algorithms for cooperative systems on-the-fly in cases where precise a priori state information is unavailable. In this paper, we provide a framework that allows estimation of the relative pose and orientation between vehicles in the presence of high initial uncertainty. Effects of cooperation among multiple vehicles exchanging estimates of heading rate and velocity and external sensor measurements are analyzed. A Multi-Hypothesis Extended Kalman Filter (MHEKF) technique is used to initialize pairwise vehicles using range-only measurements. Using solutions identified by the MHEKF algorithm, a joint filter comprising of multiple vehicles is initialized. Sufficient conditions to maintain bounded errors are derived through nonlinear observability analysis using Lie derivatives for the pairwise and the multi-vehicle cases. Using these conditions as passive constraints in the system, simulation and hardware experiments are performed to demonstrate the advantages of using MHEKF when the initial conditions are unreliable. A multi-vehicle testbed for heterogeneous platforms with different sensing modalities is developed to facilitate hardware testing. Improvements in the system performance when cooperation is introduced among vehicles is also highlighted through experiments.