Well, but it's exactly that,
why to complicate things? It's just an union of a kalman filter, an IMU and servo motors. XD