Extended Kalman Filter for Stereo Vision-Based Localization and Mapping Applications

Image feature-based localization and mapping applications useful in field robotics are considered in this paper. Exploiting the continuity of image features and building upon the tracking algorithms that use point correspondences to provide an instantaneous localization solution, an extended Kalman filtering (EKF) approach is formulated for estimation of the rigid body motion of the camera coordinates with respect to the world coordinate system. Recent results by the authors in quantifying uncertainties associated with the feature tracking methods form the basis for deriving scene-dependent measurement error statistics that drive the optimal estimation approach. It is shown that the use of certain relative motion models between a static scene and the moving target can be recast as a recursive least squares problem and admits an efficient solution to the relative motion estimation problem that is amenable to real-time implementations on board mobile computing platforms with computational constraints. The utility of the estimation approaches developed in the paper is demonstrated using stereoscopic terrain mapping experiments carried out using mobile robots. The map uncertainties estimated by the filter are utilized to establish the registration of the local maps into the global coordinate system.