Distributed collaborative localization of multiple vehicles from relative pose measurements

We propose a distributed algorithm for estimating the poses (positions and orientations) of multiple autonomous vehicles in GPS denied scenarios when pairs of vehicles can measure each other´s relative pose in their local coordinates. Currently, navigation of an autonomous vehicle in GPS denied scenarios is achieved by integrating relative pose measurements between successive time instants that are obtained from onboard sensors, such as cameras and IMUs. However, this suffers from a high rate of error growth over time. We seek methods to ameliorate this error growth by using cooperation among a group of vehicles. Measurements of relative pose between certain pairs of vehicles provide extra information on their poses, which can be used for improving localization accuracy. We designed a distributed algorithm to fuse all the relative pose measurements to compute a more accurate estimate of all the vehicles´ poses than what is possible by the vehicles individually. The algorithm is fully distributed since only neighboring vehicles need to exchange information periodically. Monte Carlo simulations show that the error in the location estimates obtained by using this algorithm is significantly lower than what is achieved when vehicles estimate their poses without cooperation.