LaneSLAM -- Simultaneous Pose and Lane Estimation Using Maps with Lane-Level Accuracy

In this paper we provide a method for coarse map localisation using low-cost sensors (GPS and camera-based lane recognition) and maps with lane-level accuracy while simultaneously updating the perceived road network and the map. This is a conceptual improvement on previous works which either focussed on a subtask (localisation or lane update) or only worked with single lanes. The problem is solved by applying Loopy Belief Propagation on a tailored factor graph which models the dependencies between observed and hidden variables. Message passing within the graph relies on multimodal normal distributions for variable representation and quadratic noise models resulting in a fast and well-defined calculation framework. Simulations show that the localisation accuracy is insensitive to most types of measurement noise except constant offsets of global pose measurements which can still be reduced by a factor of 8. Real-world tests with an average localisation error of 1.71m in an urban scenario prove the applicability of the approach for automatic driving tasks as well as its run-time performance with an average execution time of 3ms.