Bayesian Hybrid Model-State Estimation Applied to Simultaneous Contact Formation Recognition and Geometrical Parameter Estimation

In this paper we describe a Bayesian approach to model selection and state estimation for sensor-based robot tasks. The approach is illustrated with a hybrid model-state estimation example from force-controlled autonomous compliant motion: simultaneous (discrete) contact formation recognition and estimation of (continuous) geometrical parameters. Previous research in this area mostly tries to solve one of the two subproblems, or treats the contact formation recognition problem separately, avoiding integration between the solutions to the contact formation recognition and the geometrical parameter estimation problems. A more powerful hybrid model, explicitly modeling contact formation transitions, is developed to deal with larger uncertainties. This paper demonstrates that Kalman filter variants have limits: iterated extended Kalman filters can only handle small uncertainties on the geometrical parameters, while the non-minimal state Kalman filter cannot deal with model selection. Particle filters can handle the increased level of model complexity. Explicit measurement equations for the particle filter are derived from the implicit kinematic and energetic constraints. The experiments prove that the particle filter approach successfully estimates the hybrid joint posterior density of the discrete contact formation variable and the 12-dimensional, continuous geometrical parameter vector during the execution of an assembly task. The problem shows similarities with the well-known problems of data association in simultaneous localization and map-building (SLAM) and model selection in global localization.