GNSS/Multi-Sensor Fusion Using Continuous-Time Factor Graph Optimization for Robust Localization

TL;DR

This work tackles robust vehicle localization in urban environments where GNSS can be degraded or unreliable. It introduces GNSS-FGO, an online, time-centric, continuous-time factor graph framework that fuses GNSS with IMU, lidar, and speed measurements by representing the trajectory with Gaussian processes and querying states at arbitrary times. The authors compare loosely coupled and tightly coupled fusion, demonstrate robustness across challenging urban sequences, and show that GP-WNOJ priors provide accurate, smooth trajectories with configurable smoothing and computation trade-offs. The approach offers a flexible, sensor-agnostic solution that remains robust when individual sensors fail, with potential for online learning of hyperparameters and inclusion of additional modalities.

Abstract

Accurate and robust vehicle localization in highly urbanized areas is challenging. Sensors are often corrupted in those complicated and large-scale environments. This paper introduces GNSS-FGO, an online and global trajectory estimator that fuses GNSS observations alongside multiple sensor measurements for robust vehicle localization. In GNSS-FGO, we fuse asynchronous sensor measurements into the graph with a continuous-time trajectory representation using Gaussian process regression. This enables querying states at arbitrary timestamps so that sensor observations are fused without requiring strict state and measurement synchronization. Thus, the proposed method presents a generalized factor graph for multi-sensor fusion. To evaluate and study different GNSS fusion strategies, we fuse GNSS measurements in loose and tight coupling with a speed sensor, IMU, and lidar-odometry. We employed datasets from measurement campaigns in Aachen, Duesseldorf, and Cologne in experimental studies 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 the classic multi-sensor fusion method fails due to sensor degradation. In a test sequence containing a 17km route through Aachen, the proposed method results in a mean 2D positioning error of 0.48m while fusing raw GNSS observations with lidar odometry in a tight coupling.

Figures (16)

• Figure 1: Demonstration of multi-sensor fusion for vehicle localization of test sequence C02 in the city of Cologne.
• Figure 2: Continuous-time state estimation with asynchronous measurements. A time offset $\tau$ can be calculated with respect to a former state variable ${\bm x}_{i}^{}$ at timestamp $t_i$ for each asynchronous measurement. The variable $t_d$ denotes a measurement delay that is assumed to be given.
• Figure 3: A general time-centric factor graph. The state variables ${\bm x}_{t}^{}$ are created and constrained with GP motion prior factors on time while all asynchronous measurements are fused by querying a state with a time offset $\tau$ between the measurement and the former state variable. The queried states (in dashed circles) are thus not to-be-estimated state variables. We assume that the measurement delay $t_d$ is known to correct the measurement timestamp for querying a state.
• Figure 4: Coordinate frames used in this work.
• Figure 5: A general graph of loose coupling in gnssFGO.

Theorems & Definitions (5)

• Remark 1
• Remark 2
• Remark 3
• Remark 4
• Remark 5