Bug resolved & new data for setpoint topic.

Bug resolved 
 - in SO(3) estimator. Size of rotation matrix.
 - in position estimator mc.

New data for setpoint topic.
 - For quaternion-based attitude control.