Kalman Filter for 3D Motion Estimation via Lagrange Interpolation and Numerical Integration

This paper presents a Kalman filter approach for 3D motion estimation of an object that undergoes arbitrary rotational and translational motion. Problem considered here is error propagation from image features to 3D motion estimation. To this end, we have derived a new set of equations for motion estimation: (1) Lagrange interpolation and numerical integration are introduced to construct the dynamic equation, which is adaptable to the kinematics process of diverse 3D motion. (2) An adaptively estimated fading factor is imported to restrain the filter divergence caused by the truncation errors. (3) As for the measurement model, linear measurement equations relating the estimated pose parameters and the true ones are derived by exploring the relationship between the feature locations and the object pose (position and orientation) parameters. The proposed approach is suitable for real-time environment in terms of the following aspects: the structure of the filter is simplified by avoiding the need of EKF, the computational cost is much reduced by running six filters on the six motion parameters in parallel; the process to extract the feature points became much simplified due to the predicted information provided by the filter; time efficiency is increased because the sequence of image frames is allowed to be at unequal intervals. Simulation results and implement of a system for multi-mobile robots formation show the capacity of this algorithm

[1]  J. Douglas Faires,et al.  Numerical Analysis , 1981 .

[2]  Yang Yi-kang.  Constructing Estimate Model for Maneuvering Target Tracking by Numerical Differentiation: Principle and Applications , 2002 .

[3]  Narendra Ahuja,et al.  3-D Motion Estimation, Understanding, and Prediction from Noisy Image Sequences , 1987, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[4]  Rama Chellappa,et al.  Estimation of Object Motion Parameters from Noisy Images , 1986, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[5]  Steven D. Blostein,et al.  MOTION DETECTION AND ESTIMATION FROM STEREO IMAGE SEQUENCES: SOME PRELIMINARY EXPERIMENTAL RESULTS. , 1986 .

[6]  R. Chellappa,et al.  Recursive 3-D motion estimation from a monocular image sequence , 1990 .

[7]  Maurizio Ficocelli,et al.  Adaptive filtering for pose estimation in visual servoing , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[8]  Sukhan Lee,et al.  A Kalman filter approach for accurate 3-D motion estimation from a sequence of stereo images , 1991, CVGIP Image Underst..

[9]  Yikang Yang,et al.  Identification-compensation approach for unknown disturbances via the estimate model of numerical differentiation , 2002, Proceedings of the 4th World Congress on Intelligent Control and Automation (Cat. No.02EX527).

[10]  Feng Zhu,et al.  Limited memory filter for 3D relative position orientation estimation of maneuvering target , 2004, Fifth World Congress on Intelligent Control and Automation (IEEE Cat. No.04EX788).

[11]  Jake K. Aggarwal,et al.  MOTION AND TIME - VARYING IMAGERY - AN OVERVIEW. , 1986 .

[12]  Jiang Wang,et al.  3D relative position and orientation estimation using Kalman filter for robot control , 1992, Proceedings 1992 IEEE International Conference on Robotics and Automation.