3.1、姿態解算

對姿態角的測量和解算是飛行控制系統中重要的組成部分。姿態解算的速度和準確度直接影響到姿態控制效果。本文采用運算速度快，精度高的四元素互補濾波算法對姿態進行解算，能夠達到飛行控制的要求。

姿態角是通過陀螺儀、加速度計和數字羅盤等傳感器測量得到的。MEMS陀螺儀具有溫度漂移特性，長期積分運算存在累積誤差，短期數據精度高。飛行器在飛行過程中機體振動會對加速度計產生影響，同時數字羅盤是一種磁阻傳感器，很容易受到外部磁場的干擾，兩者不適合短期測量，但其測量誤差為靜態誤差，不隨時間的累加而變化，可用以長時間姿態角采集。

四元素互補濾波方法將這幾種傳感器的優勢互補。此方法的主要思想是用加速度計和數字羅盤的測量姿態修正陀螺儀的測量姿態。加速度計和數字羅盤測量出來的數據是地球重力與地球磁場在機體空間坐標上的分量，可認為是實際姿態。陀螺儀測量出來的姿態經實際姿態修正后輸出給控制器以計算控制量。歐拉角、四元素和方向余弦矩陣間的關系如式（1）所示。

根據余弦矩陣和歐拉角的定義，地理坐標系的重力向量轉到機體坐標系為第三行三個元素，即：

式（2）中，aX，aY，aZ為加速度計測量出的重力向量在各個軸向上的分量，即實際測出來的重力向量;vX，vY，vZ為陀螺積分后的姿態來推算出的重力向量，它們都是機體坐標系上的重力向量。它們之間的誤差，就是陀螺積分后的姿態和加速度計測出來的姿態之間的誤差。計算前aX，aY，aZ需規范化處理。