Graph Laplacian Extended Kalman Filter for Connected and Automated Vehicles Localization

Extended Kalman Filters have been widely applied for tracking the location of moving semi-autonomous vehicles. The latter are equipped with a multitude of sensors generating multi-modal data, while at the same time they are capable of cooperating via Vehicle-to-Vehicle communication technologies. In this paper, we have formulated a cooperative tracking scheme based on Extended Kalman Filter, in order to cope with erroneous GPS location information. It performs multi-modal fusion in a centralized and distributed manner, assuming the existence of an overall fusion center or local interaction among neighbouring and connected vehicles only. It features the property of encoding in a linear form the different measurement modalities, including range and GPS measurements, exploiting the connectivity topology of cooperating vehicles, using the graph Laplacian operator. The extended experimental evaluation using realistic vehicle trajectories extracted by CARLA autonomous driving simulator, verify the significant reduction of GPS error under various realistic conditions. Moreover, both schemes outperform existing cooperative localization methods. Finally, the distributed tracking approach exhibits similar performance and in specific cases outperforms the centralized counterpart.