Using an extended kalman filter for rigid body pose estimation

Rigid body pose is commonly represented as the rigid body transformation from one (often reference) pose to another This is usually computed for each frame of data without any assumptions or restrictions on the temporal change of the pose. The most common algorithm was proposed by Söderkvist and Wedin (1993, "Determining the Movements of the Skeleton Using Well-configured Markers," J. Biomech., 26, pp. 1473-1477), and implies the assumption that measurement errors are isotropic and homogenous. This paper describes an alternative method based on a state space formulation and the application of an extended Kalman filter (EKF). State space models are formulated, which describe the kinematics of the rigid body. The state vector consists of six generalized coordinates (corresponding to the 6 degrees of freedom), and their first time derivatives. The state space models have linear dynamics, while the measurement function is a non-linear relation between the state vector and the observations (marker positions). An analytical expression for the linearized measurement function is derived. Tracking the rigid body motion using an EKF enables the use of a priori information on the measurement noise and type of motion to tune the filter. The EKF is time variant, which allows for a natural way of handling temporarily missing marker data. State updates are based on all the information available at each time step, even when data from fewer than three markers are available. Comparison with the method of Söderkvist and Wedin on simulated data showed a considerable improvement in accuracy with the proposed EKF method when marker data was temporarily missing. The proposed method offers an improvement in accuracy of rigid body pose estimation by incorporating knowledge of the characteristics of the movement and the measurement errors. Analytical expressions for the linearized system equations are provided, which eliminate the need for approximate discrete differentiation and which facilitate a fast implementation.