Distributed asynchronous cooperative localization with inaccurate GNSS positions

Localization remains a major issue for autonomous vehicles. Accurate localization relative to the road and other vehicles is essential for many navigation tasks. When vehicles cooperate and exchange information through wireless communications, they can improve mutually their localization. This paper presents a distributed cooperative localization method based on the exchange of Local Dynamic Maps (LDMs). Every LDM contains dynamic information on the pose and kinematic of all the cooperating agents. Different sources of information such as dead-reckoning from the CAN bus, inaccurate (i.e. biased) GNSS positions, LiDAR and road border detections are merged using an asynchronous Kalman filter strategy. The LDMs received from the other vehicles are merged using a Covariance Intersection Filter to avoid data incest. Experimental results are evaluated on platooning scenarios. They show the importance of estimating GNSS biases and having accurate relative measurements to improve the absolute localization process. These results also illustrate that the relative localization between vehicles is improved in every LDMs even for vehicles not able to perceive surrounding vehicles but which are instead perceived by others.