Sensor Fusion for Self-Localisation of Automated Vehicles

Accurate pose estimation is key for a large number of real world applications. For example, automated cars require fast, recent, accurate, and highly available pose estimates for robust operation. Multiple redundant and complementary localisation systems are therefore installed on most automated vehicles. This work proposes a novel multi-sensor pose fusion approach for generically combining measurements from multiple localisation systems into a single pose estimate. We formulate our approach as a sliding window pose graph and enforce a particular chain graph structure, which enables efficient optimisation and a novel form of marginalisation. Our pose fusion approach scales from a filtering-based to a batch solution by increasing the size of the sliding window. It also adapts online to the available computational resources to guarantee high availability. We evaluate our approach on simulated data as well as on real data gathered with a prototype vehicle and demonstrate that our solution runs at 20 Hz, provides timely estimates, is accurate, and yields high availability.