A Monocular Vision-aided Inertial Navigation System with Improved Numerical Stability

Presented at the AIAA Guidance Navigation and Control Conference, Kissimmee, Florida, January 2015.This paper develops a monocular vision-aided inertial navigation system based on the factored extended Kalman filter (EKF) proposed by Bierman and Thornton. The simultaneous localization and mapping (SLAM) algorithm measurement update and propagation steps are formulated in terms of the factored covariance matrix P = UDUT, and a novel method for efficiently adding and removing features from the covariance factors is presented. The system is compared to the standard EKF formulation in navigation performance and computational requirements. The proposed method is shown to improve numerical stability with minimal impact on computational requirements. Flight test results are presented which demonstrate navigation performance with a controller in the loop