Resilient Factor Graph-Based GNSS/IMU/Vision/Odo Integrated Navigation Scheme Enhanced by Noise Approximate Gaussian Estimation in Challenging Environments

The signal blockage and multipath effects of the Global Navigation Satellite System (GNSS) caused by urban canyon scenarios have brought great technical challenges to the positioning and navigation of autonomous vehicles. In this paper, an improved factor graph optimization algorithm enhanced by a resilient noise model is proposed. The measurement noise is resilient and adjusted based on an approximate Gaussian distribution-based estimation. In estimating and adjusting the noise parameters of the measurement model, the error covariance matrix of the multi-sensor fusion positioning system is dynamically optimized to improve the system accuracy. Firstly, according to the approximate Gaussian statistical property of the GNSS/odometer velocity residual sequence, the measured data are divided into an approximate Gaussian fitting region and an approximate Gaussian convergence region. Secondly, the interval is divided according to the measured data, and the corresponding variational Bayesian network and Gaussian mixture model are used to estimate the innovation online. Further, the noise covariance matrix of the adaptive factor graph-based model is dynamically optimized using the estimated noise parameters. Finally, based on low-cost inertial navigation equipment, GNSS, odometer, and vision, the algorithm is implemented and verified using a simulation platform and real-vehicle road test. The experimental results show that in a complex urban road environment, compared with the traditional factor graph fusion localization algorithm, the maximum improvement in accuracy of the proposed algorithm can reach 65.63%, 39.52%, and 42.95% for heading, position, and velocity, respectively.