GNSS/Multisensor Fusion Using Continuous-Time Factor Graph Optimization for Robust Localization
Haoming Zhang, Chih-Chun Chen, Heike Vallery, Timothy D. Barfoot
Abstract
Accurate and robust vehicle localization in highly urbanized areas is challenging. Sensors are often corrupted in those complicated and large-scale environments. This article introduces <monospace xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">gnssFGO</monospace>, a global and online trajectory estimator that fuses global navigation satellite systems (GNSS) observations alongside multiple sensor measurements for robust vehicle localization. In <monospace xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">gnssFGO</monospace>, we fuse asynchronous sensor measurements into the graph with a continuous-time trajectory representation using Gaussian process (GP) regression. This enables querying states at arbitrary timestamps without strict state and measurement synchronization. Thus, the proposed method presents a generalized factor graph for multisensor fusion. To evaluate and study different GNSS fusion strategies, we fuse GNSS measurements in loose and tight coupling with a speed sensor, inertial measurement unit, and LiDAR-odometry. We employed datasets from measurement campaigns in Aachen, Düsseldorf, and Cologne and presented comprehensive discussions on sensor observations, smoother types, and hyperparameter tuning. Our results show that the proposed approach enables robust trajectory estimation in dense urban areas where a classic multisensor fusion method fails due to sensor degradation. In a test sequence containing a 17-km route through Aachen, the proposed method results in a mean 2-D positioning error 0.48 m while fusing raw GNSS observations with LiDAR odometry in a tight coupling