A square root unscented Kalman filter for multiple view geometry based stereo cameras/inertial navigation

Abstract

Exact motion estimation is one of the major tasks in autonomous navigation. Conventional Global Positioning System-aided inertial navigation systems are able to provide accurate locations. However, they are limited when used in a Global Positioning System-denied environment. In this paper, we present a square root unscented Kalman filter-based approach for navigation by using stereo cameras and an inertial sensor only. The main contribution of this work is the development of a novel measurement model by applying multiple view geometry constraints to the stereo cameras/inertial system. The measurement model does not require the three-dimensional feature position in the state vector of the filter, which substantially reduces the size of the state vector and the computational burden. To incorporate this nonlinear and complex measurement model, a variant of the square root unscented Kalman filter-based algorithm is also proposed. The root of the state covariance is propagated and updated directly in the square root unscented Kalman filter, thereby avoiding the decomposition of the state covariance and improving the stability of our algorithm. Experimental results based on a real outdoor dataset are presented to demonstrate the feasibility and the accuracy of the proposed approach.