Loosely coupled Kalman filtering for fusion of Visual Odometry and inertial navigation

Visual Odometry (VO) is the process of estimating the motion of a system using single or stereo cameras. Performance of VO is comparable to that of wheel odometers and GPS under certain conditions; therefore it is an accepted choice for integration with inertial navigation systems especially in GPS denied environments. In general, VO is integrated with the inertial sensors in a state estimation framework. Despite the various instances of estimation filters, the underlying concepts remain the same, an assumed kinematic model of the system is combined with measurements of the states of that system. The drawback of using kinematic models for state transition is that the state estimate will only be as good as the precision of the model used in the filter. A common approach in navigation community is to use an error propagation model of the navigation solution using inertial sensor instead of an assumed dynamical model. High rate IMU will trace the dynamic better than an assumed model. In this paper, we propose a loosely coupled indirect feedback Kalman filter integration for visual odometry and inertial navigation system that is based on error propagation model and takes into account different characteristics of individual sensors for optimum performance, reliability and robustness. Two measurement models are derived for the accumulated and incremental visual odometry measurements. A practical measurement model approach is proposed for the delta position and attitude change measurements that inherently includes delayed-state. The non-Gaussian, non-stationary and correlated error characteristics of VO, that is not suitable to model in a standard Kalman filter, is tackled with averaging the measurements over a Kalman period and utilizing a sigma-test within the filter.