Relative pose estimation from points by Kalman filters

This paper addresses a method of dynamically estimating relative pose between two rigid bodies from four corresponding point sets during motion in applications such as visual servoing and metrology for intelligent manufacturing. Instead of tracking each body individually relative to a camera, the underlying problem is how to track two bodies simultaneously and accurately. For this reason, the proposed method directly assigns the relative pose and motion as a state estimate. Further, an observation model is formulated in such a way that the state estimate is mapped to the observed point sets of two bodies. Since this observation model is nonlinear, an analytical expression of linearization is derived and state estimation by iterative extended Kalman filters (IEKF) is adapted to reduce the linearization errors. The proposed approach is implemented in simulations of position-based visual servoing and validated experimentally on a robotic riveting system for aircraft automated assembly.