從陀螺儀器的三軸角速度通過四元數(shù)法得到俯仰，航偏，滾轉(zhuǎn)角，這是快速解算，結(jié)合三軸地磁和三軸加速度得到漂移補(bǔ)償和深度解算。

　　姿態(tài)的數(shù)學(xué)模型坐標(biāo)系

　　姿態(tài)解算需要解決的是四軸飛行器和地球的相對(duì)姿態(tài)問題。地理坐標(biāo)系是固定不變的，正北，正東，正上構(gòu)成了坐標(biāo)系的X，Y，Z軸用坐標(biāo)系R表示，飛行器上固定一個(gè)坐標(biāo)系用r表示，那么我們就可以適用歐拉角，四元數(shù)等來描述r和R的角位置關(guān)系。

　　姿態(tài)的數(shù)學(xué)表示

　　姿態(tài)有多種數(shù)學(xué)表示方式，常見的是四元數(shù)，歐拉角，矩陣和軸角。在四軸飛行器中使用到了四元數(shù)和歐拉角，姿態(tài)解算的核心在于旋轉(zhuǎn)。姿態(tài)解算中使用四元數(shù)來保存飛行器的姿態(tài)，包括旋轉(zhuǎn)和方位。在獲得四元數(shù)之后，會(huì)將其轉(zhuǎn)化為歐拉角，然后輸入到姿態(tài)控制算法中。姿態(tài)控制算法的輸入?yún)?shù)必須要是歐拉角。AD值是指MPU6050的陀螺儀和加速度值，3個(gè)維度的陀螺儀值和3個(gè)維度的加速度值，每個(gè)值為16位精度。AD值必須先轉(zhuǎn)化為四元數(shù)，然后通過四元數(shù)轉(zhuǎn)化為歐拉角。在四軸上控制流程如下圖：

　　下面是用四元數(shù)表示飛行姿態(tài)的數(shù)學(xué)公式，從MPU6050中采集的數(shù)據(jù)經(jīng)過下面的公式計(jì)算就可以轉(zhuǎn)換成歐拉角，傳給姿態(tài)PID控制器中進(jìn)行姿態(tài)控制。