Simultaneous state and dynamics estimation in articulated structures

Given an articulated rigid body, we define the problem of estimating its dynamics as the problem of computing all the forces and accelerations acting on the bodies which constitute the articulated system. Similarly, we define the state estimation problem as the problem of computing the system positions and velocities. In the present paper we propose a framework for simultaneous state and dynamics estimation. The estimation is framed in a Bayesian framework and a suitable Bayesian prior is defined to guarantee the physical consistency of the obtained estimation. The Bayesian posterior makes use of all available measurements which include encoders, gyroscopes, accelerometers, force and torque sensors. The proposed theoretical framework is validated both on simulation and on the iCub humanoid. The software that implements the theoretical framework is realised with an open-source license.