Physically-consistent sensor fusion in contact-rich behaviors

We describe an accurate approach to state estimation which fuses any available sensor data with physical consistency priors. This is done by combining the advantages of recursive estimation and fixed-lag smoothing: at each step we re-estimate the trajectory over a time window into the past, but also use a recursive prior obtained from the previous time step via internal simulation. We also incorporate a physics engine into the estimator, which makes it possible to adjust the state estimates so that the inferred contact interactions are consistent with the observed accelerations. The estimator can utilize contact sensors to improve accuracy, but even in the absence of such sensors it reasons correctly about contact forces. Estimation speed and accuracy are demonstrated on a 28-DOF humanoid robot (Darwin) in a walking task. Timing tests and leave-one-out cross-validation show that the proposed approach can be used in real-time and is substantially more accurate than the EKF, without any over-fitting. A video of our results is attached.