Graph based vehicle infrastructure cooperative localization

This paper presents a novel and an improved approach for estimating the position of a vehicle using vehicle-infrastructure cooperative localization. In our previous work we presented a Factor Graph based solution which added the topology (inter-vehicle distance) as a constraint while localizing the vehicle using data from sensors from both inside and outside the vehicle. This paper extends the work by reducing the error in calculating the precision of the position by almost 27% in the best case and lowering the computational time by at least 50% over our previously proposed solution. This is achieved by modifying current topology constraints to be also dependent on the previous state estimate. The proposed solution remains scalable for many vehicles without increasing the execution complexity. Finally, simulations indicate that incorporating the new topology information via Factor Graphs can improve performance over the traditional, state of the art, Kalman Filter approach.