Factored Extended Kalman Filter for Monocular Vision-Aided Inertial Navigation

This paper describes a novel visual simultaneous localization and mapping system based on the UD factored extended Kalman filter. A novel method for marginalizing and initializing state variables is presented, allowing for features to be added and removed directly to and from the covariance factors. An analysis of the number of operations in the marginalization and initialization algorithm are presented. A randomized analysis demonstrates the improvement in numerical stability over the standard and Joseph-form extended Kalman filter by as much as three orders of magnitude. The navigation system is implemented on an 80 kg unmanned aerial vehicle and a 0.5 kg unmanned aerial vehicle, and flight-test and simulation results with a controller in the loop are presented. The flight-test results agree with the simulation results and show a low state error and a consistent error covariance.