Decoupled state estimation for humanoids using full-body dynamics

We propose a framework to use full-body dynamics for humanoid state estimation. The main idea is to decouple the full body state vector into several independent state vectors. Some decoupled state vectors can be estimated very efficiently with a steady state Kalman Filter. In a steady state Kalman Filter, state covariance is computed only once during initialization. Furthermore, decoupling speeds up numerical linearization of the dynamic model. We demonstrate that these state estimators are capable of handling walking on flat ground and on rough terrain.