State estimation for dynamic systems with intermittent contact

Dynamic system states estimation, such as object pose and contact states estimation, is essential for robots to perform manipulation tasks. In order to make accurate estimation, the state transition model needs to be physically correct. Complementarity formulations of the dynamics are widely used for describing rigid body physical behaviors in the simulation field, which makes it a good state transition model for dynamic system states estimation problem. However, the non-smoothness of complementarity models and the high dimensionality of the dynamic system make the estimation problem challenging. In this paper, we propose a particle filtering framework that solves the estimation problem by sampling the discrete contact states using contact graphs and collision detection algorithms, and by estimating the continuous states through a Kalman filter. This method exploits the piecewise continuous property of complementarity problems and reduces the dimension of the sampling space compared with sampling the high dimensional continuous states space. We demonstrate that this method makes stable and reliable estimation in physical experiments.