Robust cooperative localization in a dynamic environment using factor graphs and probability data association filter

Autonomous vehicles operating in dynamic environments rely on precise localization. In this paper we present a novel approach for cooperative localization of vehicular systems and an infrastructure RADAR which is resilient against outliers generated from the RADAR. The problem of cooperative localization is represented as a factor graph, where interrelated topologies (including that of outliers) are added as constraint factor between vehicle states. Corresponding probabilities for multiple topologies between states of the two vehicles are calculated using the Probability Data Association Filter and assigned to the respective edges in the graph. Simulation results indicate that this technique has significant benefits in the context of improving the resilience against outliers while optimizing joint state estimates. The methodology presented in this paper has the potential to provide a robust and flexible framework for cooperative localization in the presence of clutter, obscuration and targets entering and leaving the field of view.