[estimation][control] SO3 attitude control

[sugikazu75]

What is this

#548 is recreated from current master branch.

[tongtybj]

@sugikazu75

I have came up with the solution of replacing the gavity direciton with quaternion for the imu topic from spinal to ROS. I will implement it in few days.

[sugikazu75]

@sugikazu75

I have came up with the solution of replacing the gavity direciton with quaternion for the imu topic from spinal to ROS. I will implement it in few days.

Ok, I think it's better way.
Will quaternion be estimated by imu and magnetometer, right?

[sugikazu75]

memo: I had to calibrate magnetometer before merging 7b8a8cc

[sugikazu75]

memo: I had to calibrate magnetometer before merging 7b8a8cc

I solved this by 32db881

[tongtybj]

@sugikazu75

I forgot to tell you that I am refactoring this branch.
So please pause the further development on this branch, and create another temporal branch for any other patch.
Could you revert to 47e6c27 for this branch?