https://blog.csdn.net/u012700322/article/details/52252305
http://blog.sina.com.cn/s/blog_ab603ca00101m9mc.html
https://www.cnblogs.com/21207-iHome/p/6894128.html
四元数转航向角,使用roll,yaw,pitch分别表示物体绕x,y,z的旋转角度,记为:
则:
当绕z轴旋转角度a时,x=0,y=0,z=sin(a/2),w=cos(a/2).