Pose fusion with chain pose graphs for automated driving

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.