A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles

This paper describes a robust integrated positioning method to provide ground vehicles in urban environments with accurate and reliable localization results. The localization problem is formulated as a maximum a posteriori probability estimation and solved using graph optimization instead of Bayesian filter. Graph optimization exploits the inherent sparsity of the observation process to satisfy the real-time requirement and only updates the incremental portion of the variables with each new incoming measurement. Unlike the Extended Kalman Filter (EKF) in a typical tightly coupled Global Navigation Satellite System/Inertial Navigation System (GNSS/INS) integrated system, optimization iterates the solution for the entire trajectory. Thus, previous INS measurements may provide redundant motion constraints for satellite fault detection. With the help of data redundancy, we add a new variable that presents reliability of GNSS measurement to the original state vector for adjusting the weight of corresponding pseudorange residual and exclude faulty measurements. The proposed method is demonstrated on datasets with artificial noise, simulating a moving vehicle equipped with GNSS receiver and inertial measurement unit. Compared with the solutions obtained by the EKF with innovation filtering, the new reliability factor can indicate the satellite faults effectively and provide successful positioning despite contaminated observations.