The large error of state estimator

[hxydebug]

Sorry to bother you. Right now I am using this your project to estimate my biped robot. But I found there are many errors of my estimated orientation and COM position in world frame. What can I do to decrease the error? Is the initial guess very important for my problem? Meanwhile, I found the CoM position in z direction is drifting. Is it because this code do not set z = 0 when foot contact the ground?