SO(3) estimator has been debugged and cleaned.

This is clean version of so3 estimator tested.
Also, quaternion data display on QGC is implemented when only q_valid is true.