Tightly-Coupled Image-Aided Inertial Navigation Using the Unscented Kalman Filter

摘要

Accurate navigation information (position, velocity, and attitude) can be determined using optical measurements from imaging sensors combined with an inertial navigation system. This can be accomplished by tracking the locations of stationary optical features in multiple images and using the resulting geometry to estimate and remove inertial errors. In previous research efforts, we have demonstrated the effectiveness of fusing imaging and inertial sensors using an extended Kalman filter (EKF) algorithm. In this approach, the image feature correspondence search was aided using the inertial sensor measurements, resulting in more robust feature tracking. The resulting image-aided inertial algorithm was tested using both simulation and experimental data. While the tightly-coupled approach stabilized the feature correspondence search, the overall problem remained prone to filter divergence due to the well-known consequences of image scale ambiguity and the nonlinear measurement model. These effects are evidenced by the consistency divergence in the EKF implementation seen during our long-duration Monte-Carlo simulations. In other words, the measurement model is highly sensitive to the current parameter estimate, which invalidates the linearized measurement model assumed by the EKF. The unscented (sigma-point) Kalman filter (UKF) has been proposed in the literature in order to address the large class of recursive estimation problems which are not well-modeled using linearized dynamics and Gaussian noise models assumed in the EKF. The UKF leverages the unscented transformation in order to represent the state uncertainty using a set of carefully chosen sample points. This approach maintains mean and covariance estimates accurate to at least second order, by using the true nonlinear dynamics and measurement models. In this paper, a variation of the UKF is applied to the image-aided inertial navigation problem, with the goal of improving upon the established limitations of our previous EKF implementation. A tightly-coupled image-aided inertial UKF is rigorously designed from first principles. The UKF is evaluated using a combination of simulated and experimental data. The performance of the image-aided navigation system is analyzed and compared to the baseline EKF from our previous work.