Multi body Kalman filtering with articulation constraints for humanoid robot pose and motion estimation

In this paper, a concept for articulated rigid body state estimation is proposed. The articulated body, for instance a humanoid robot, is modeled in a maximal coordinate formulation and the articulations between the rigid bodies as nonlinear position and linear motion constraints. At first, the individual state of each particular rigid body is estimated with a Kalman filter, which leads to an unconstrained state estimate. Subsequently, the correct state estimate for the articulated rigid body is derived by projecting the unconstrained estimate onto the constraint surface.