# Three-dimensional motion parameter estimation for rigid bodies using an iterated extended Kalman filter

A method based on the Kalman filter is described for estimating the three dimensional motion parameters, i.e. position, translational velocity, translational acceleration, orientation and rotational velocity, of an opaque, three dimensional rigid object under certain types of motion. This method has applications in industrial robotics, surveillance and smart highways that are currently under investigation in Europe. For this method, it is assumed that the image plane locations of a set of features are available over a large sequence of images and that the three dimensional location of these features with respect to an object centered coordinate system is known. Furthermore, it is assumed that the object motion is described as rotation about an axis that passes through the origin of the object centered coordinate system and translation of the object centered coordinate system Because both the plant and the measurement models are nonlinear, an Extended Kalman filter and the Iterated Kalman filter are used. The state vector is composed of the three dimensional location, translational velocity, translational acceleration, the three dimensional rotational velocity and quaternions describing the orientation of the axis of rotation. The measurements are the image plane coordinates of a set of known features. The perspective projection is used in deriving the measurement model. To account for modelling inaccuracies, zero mean white Gaussian noise is added to the plant model Since the object is assumed to be non-transparent, only the visible features from the set of feature points are used, thus eliminating the need to assume that all elements of the feature set are always visible. Occlusion of features due to parts of the object itself or any other passing object is thus addressed in the framework of the Extended and the Iterated Extended Kalman filter scheme. An extension to the model demonstrates the use of this approach to estimating motion parameters of multiple moving objects All experiments performed to test for validity, performance and robustness of this approach are done using simulations. Using simulation as a tool it is shown that this approach performs admirably well for pure translation, pure rotation and for objects undergoing both translation and rotation in all three dimensions even when all the features of the object are not visible in every image frame of the image sequence. Using the same tools it is shown that this approach performs adequately in cases where the object motion deviates from the plant model, in estimating motion parameters of more than one object in the field of view