[Modle][Control][Navigation] refactor the whole system from XYZ-Euler angles to true SO(3)

[tongtybj]

What is this

Handle the robot orientation with SO(3) rotation matrix. So far, the orientation is described in XYZ-Euler angles, and the singularity
leads the limitation to handle omni-directional motion for our robots.

Details

• The orientation is mangaed by using rotation matrix in both spinal and ros.
• Spinal publishes quaternion instead of euler angles to ensure the consistence of SO(3) orientation
• Devide rotation matrix into three orthogonal unit vectors (wx_b, wy_b, wz_b): wz_b is solely related to gravity, and thus it is possible to estimated from accelerometer and gyrometer. While wx_b can be obtaind from magnetometer and gyrometer ( and a assistance of wz_b). Them, wy_b can be calcualted from the cross-production of wx_b and wz_b. This is the bacis concept to obtain rotation matrix using 9-axis IMU in Spinal. Also refer to the Fig.1
• A smater fusion between VIO/Mocap and Spinal IMU in terms of orientation: rotation matrix from VIO/Mocap can be also be devided into wx_b, wy_b and wz_b. Then wx_b can be soley used to update rotation matrix. Please check these API and usage in codes. Fig.2 can be also helpful for understanding.

Fig.1

Fig.2

TODO

• Test with simulation (hydrus, dragon)
• Test with rosbag (mini quadrotor)
• Test with in-flight experiment (mini quadrotor)

[tongtybj]

@sugikazu75

This is the latest progress of SO(3) orientation system.