Numerically and statistically stable Kalman filter for INS/GNSS integration

This paper addresses the numerical and statistical stability in the integration of inertial navigation system and global navigation satellite system (GNSS) using Kalman filter (KF). Due to the different units used in horizontal and vertical components of the geodetic (curvilinear) coordinates in the vicinity of the earth’s surface, i.e. radians in the former and meters in the latter, the covariance of the innovation vector in KF, which should be inversed, can be ill-conditioned, resulting in severe numerical instability. A simple rescaling method is proposed to address this problem, specifically, the horizontal components of the positioning error, i.e. the latitude and longitude errors are rescaled by multiplying the average radius of the earth and hence the covariance of the state, and the state space model should be modified accordingly. This method is applicable on most of the earth surface except for two small regions near the poles. Due to the multipath and/or jamming effect under some adverse circumstances, the GNSS measurements are prone to outliers which will degrade the KF’s performance severely. A robust refinement to the KF based on Chi-square test and covariance inflation is proposed to make the integration outlier-resistant, and specifically, in the presence of GNSS measurements, a Chi-square test is carried out to detect outlier, and once detected, the outlier is less weighted by inflating the covariance of the innovation vector, and the inflating factor is solved in closed form. Simulation results validate the efficacy of the proposed method.