Debuging of estimator quaternion - not working yet