A General Framework for Multi-vehicle Cooperative Localization Using Pose Graph

When a vehicle observes another one, the two vehicles' poses are correlated by this spatial relative observation, which can be used in cooperative localization for further increasing localization accuracy and precision. To use spatial relative observations, we propose to add them into a pose graph for optimal pose estimation. Before adding them, we need to know the identities of the observed vehicles. The vehicle identification is formulated as a linear assignment problem, which can be solved efficiently. By using pose graph techniques and the start-of-the-art factor composition/decomposition method, our cooperative localization algorithm is robust against communication delay, packet loss, and out-of-sequence packet reception. We demonstrate the usability of our framework and effectiveness of our algorithm through both simulations and real-world experiments using three vehicles on the road