Switchable constraints and incremental smoothing for online mitigation of non-line-of-sight and multipath effects

Reliable vehicle positioning is a crucial requirement for many applications of advanced driver assistance systems. While satellite navigation provides a reasonable performance in general, it often suffers from multipath and non-line-of-sight errors when it is applied in urban areas and therefore does not guarantee consistent results anymore. Our paper proposes a novel online method that identifies and excludes the affected pseudorange measurements. Our approach does not depend on additional sensors, maps, or environmental models. We rather formulate the positioning problem as a Bayesian inference problem in a factor graph and combine the recently developed concept of switchable constraints with an algorithm for efficient incremental inference in such graphs. We furthermore introduce the concepts of auxiliary updates and factor graph pruning in order to accelerate convergence while keeping the graph size and required runtime bounded. A real-world experiment demonstrates that the resulting algorithm is able to successfully localize despite a large number of satellite observations are influenced by NLOS or multipath effects.