A Kalman filter approach for accurate 3-D motion estimation from a sequence of stereo images

Abstract

This paper presents a Kalman filter approach for accurately estimating the 3-D position and orientation of a moving object from a sequence of stereo images. Emphasis is given to finding a solution for the following problem incurred by the use of a long sequence of images: the images taken from a longer distance suffer from a larger noise-to-signal ratio, which results in larger errors in 3-D reconstruction and, thereby, causes a serious degradation in motion estimation. To this end, we have derived a new set of discrete Kalman filter equations for motion estimation: (1) The measurement equation is obtained by analyzing the effect of white Gaussian noise in 2-D images on 3-D positional errors (instead of directly assigning Gaussian noise to 3-D feature points) and by incorporating an optimal 3-D reconstruction under the constraints of consistency satisfaction among 3-D feature points. (2) The state propagation equation, or the system dynamic equation, is formulated by describing the rotation between two consecutive 3-D object poses, based on quaternions and representing the error between the true rotation and the nominal rotation (obtained by 3-D reconstruction) in terms of the measurement noise in 2-D images. Furthermore, we can estimate object position from the estimation of object orientation in such a way that an object position can be directly computed once the estimation of an object orientation is obtained. Simulation results indicate that the Kalman filter equations derived in this paper represent an accurate model for 3-D motion estimation in spite of the first-order approximation used in the derivation. The accuracy of this model is demonstrated by the significant error reduction in the presence of large triangulation errors in a long sequence of images and by a shorter transition period for convergence to the true values.