Robust Vehicle Infrastructure Cooperative Localization in Presence of Clutter

One of the primary challenges for a successful Highly Assisted and/or Autonomous Vehicle is its localization. To improve the precision of location of the vehicle, not only the internal sensors are being used, but also using data from external sensors is attracting increasing attention from the research community. One such proposed sensor is an infrastructure RADAR which can be used to improve the localization of the ego-vehicle. Although a RADAR indeed is a supplementary source of information, it suffers a unique type of clutter which have trajectories like real objects and can therefore result in "ghost measurements", i.e., measurements which do not correspond to any real vehicles. This deteriorates the quality of the fused state estimates. This paper proposes a robust method to fuse the RADAR readings in presence of such outliers. This methodology builds upon a previously proposed solution where the problem was formulated as a factor graph. The RADAR measurements were added as a novel constraint of sum of inter-vehicle distance, called Topology Factor. Our previous work assumed clutter free environment. This paper proposes a novel robust Topology Factor which is also resilient against above mentioned outliers. Simulations (based on real data) show promising results in the direction of lowering the degradation of fused state estimates in presence of such clutter.