Vehicle infrastructure cooperative localization using Factor Graphs

Highly assisted and Autonomous Driving is dependent on the accurate localization of both the vehicle and other targets within the environment. With increasing traffic on roads and wider proliferation of low cost sensors, a vehicle-infrastructure cooperative localization scenario can provide improved performance over traditional mono-platform localization. The paper highlights the various challenges in the process and proposes a solution based on Factor Graphs which utilizes the concept of topology of vehicles. A Factor Graph represents probabilistic graphical model as a bipartite graph. It is used to add the inter-vehicle distance as constraints while localizing the vehicle. The proposed solution is easily scalable for many vehicles without increasing the execution complexity. Finally simulation indicates that incorporating the topology information as a state estimate can improve performance over the traditional Kalman Filter approach.