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