Real-Time State Estimation With Whole-Body Multi-Contact Dynamics: A Modified Ukf Approach

We present a real-time state estimator applicable to whole-body dynamics in contact-rich behaviors. Our estimator is based on the Unscented Kalman Filter (UKF), with modifications that proved essential in the presence of strong contact non-linearities combined with unavoidable model errors. Instead of the standard UKF weighting scheme, we use uniform weighting which is less susceptible to samples that violate unilateral constraints. We also develop a rich model of process noise including noise in system parameters, control noise, as well as a novel form of timing noise which makes the estimator more robust. The method is applied to an enhanced Darwin robot in walking as well as pseudo-walking while lying on the floor. Estimation accuracy is quantified via leave-one-out cross-validation, as well as comparisons to ground-truth motion capture data which is not used by the estimator. A full update takes around 7 msec on a laptop processor. Furthermore we perform the computationally-expensive prediction step (involving 210 forward dynamics evaluations in the MuJoCo simulator) while waiting for sensor data, and then apply the correction step as soon as sensor data arrive. This correction only takes 0.5 msec. Thus the estimator adds minimal latency to closed-loop control, even though it handles the whole-body robot dynamics directly, without resorting to any of the modeling shortcuts used in the past.