FGO-MFI: factor graph optimization-based multi-sensor fusion and integration for reliable localization

Abstract

Reliable and precise information pertaining to the position, velocity, and attitude is essential for automated driving. This paper proposes FGO-MFI, a cost-effective and robust multi-sensor fusion and integration localization framework that utilizes factor graph optimization. Firstly, a tightly coupled GNSS (Global Navigation Satellite Systems)/on-board sensor fusion localization framework is established to estimate vehicle states, including position, velocity, and attitude. To address the large drift rate of the IMU (Inertial Measurement Unit), this study introduces a novel IMU/Dynamics pre-integration method based on the vehicle dynamics model. We establish a two-degree-of-freedom vehicle dynamics model utilizing measurements from the wheel speed sensor and steering wheel angle sensor. The IMU/Dynamics factor is devised through a close integration of the model output and IMU pre-integration, enabling the construction of precise odometry with low-cost on-board sensors. Then, to address the issue of the non-Gaussian distribution of GNSS pseudorange error, this paper employs a GMM (Gaussian Mixture Model) to characterize the pseudorange noise, which is then applied to further sensor fusion. Given the time-varying nature of pseudorange noise, the EM (expectation maximization) algorithm is utilized to estimate GMM parameters online, leveraging the pseudorange residuals within a sliding window. Comprehensive experiments, inclusive of challenging scenarios such as urban canyons, tunnels, and wooded areas, have been carried out. They affirm the superior performance of the proposed method. Experiments have shown that our method demonstrates reliable localization across different statuses of GNSS signal, exhibiting a 39.4% improvement in the root mean square error of position error when compared to the state-of-the-art. Additionally, this FGO-MFI is a general sensor data fusion framework and is able to incorporate diverse sensor measurements, for example, from cameras and LiDARs to provide more reliable and accurate localization information.