Continuous-Time Factor Graph Optimization for Trajectory Smoothness of GNSS/INS Navigation in Temporarily GNSS-Denied Environments

Abstract

For autonomous systems, the smoothness of estimated trajectories is essential to ensure robust and performant vehicle control. Unfortunately, approaches for state estimation using Kalman filters are sensitive to measurement outliers that can diverge dramatically. We propose a state-estimation algorithm using factor graph optimization (FGO) in continuous-time for GNSS/INS navigation systems to track this problem. The focus is on the smoothness of the estimated trajectory in environments where the GNSS observations become temporarily unreliable. Therefore, an inland shipping scenario with bridge crossings is selected as an application example. To estimate the trajectory in continuous-time, we integrate a White-Noise-On-Acceleration (WNOA) motion prior factor based on Gaussian process regression between successive states. Our results show a 30% improvement in accuracy and increased smoothness of the estimated trajectory with FGO compared to Kalman filtering.