This is a code dump of a previous implementation of the following papers
Rotella, Nicholas, et al.
"State estimation for a humanoid robot."
Intelligent Robots and Systems (IROS 2014),
2014 IEEE/RSJ International Conference on. IEEE, 2014.
Bloesch, Michael, et al.
"State estimation for legged robots-consistent fusion of leg kinematics and IMU."
Robotics 17 (2013): 17-24.
It has dependencies which I have not included. But it should be readable.