Localizing a vehicle in a map is essential for automated driving and various other robotic applications. This paper addresses the problem of vehicle localization in urban environments. Our approach performs a graph-based sliding window optimization over a set of recent landmark and odometry measurements for fast and accurate vehicle localization on third-party maps. Our work incorporates landmark priors from third-party maps into the estimation problem and shows how to exploit the sliding window formulation for revising data associations. We describe how to construct our factor graph and derive its necessary factors to model the information from the map as a prior over the landmark detections. We implemented our approach on an automated car and thoroughly tested it on real-world data. The experiments suggest that the approach provides highly accurate pose estimates, is fast enough for automated driving applications, and outperforms localization using particle filters.

Automated driving relies on fast, recent, accurate, and highly available pose estimates. A single localization system, however, can commonly ensure this only to some extent. In this paper, we propose a multi-sensor fusion approach that resolves this by combining multiple localization systems in a plug and play manner. We formulate our approach as a sliding window pose graph and enforce a particular graph structure which enables efficient optimization and a novel form of marginalization. Our pose fusion approach scales from a filtering-based to a batch solution by increasing the size of the sliding window. We evaluate our approach on simulated data as well as on real data gathered with a prototype vehicle and demonstrate that our solution runs comfortably at 20 Hz, provides timely estimates, is accurate, and yields a high availability.