方向餘弦矩陣
飛行器在空中的運行姿態可以用平面和轉動來表示，爲了方便使用向量表示，需要建立兩個空間直角座標系。設R表示單位矢量在機體座標系下的三個軸的投影，B表示單位矢量在地球座標系下的三個軸的投影。我們通常對飛行器的偏航、俯仰、橫滾了多少度的定義是參照地球座標系而下得出的，也就是我們只需要知道矢量R相較於矢量B進行了怎樣的運動就能獲取所需的姿態信息。而向什麼方向進行怎樣的運動就是方向餘弦矩陣。現設C_B^R表示矢量由座標系B到座標系R的方向餘弦矩陣則R=C_B^R B。
直接把公式貼圖了，word裏面弄過來就變成上面文本的形狀了
方向餘弦矩陣就是表示將一個座標系轉化到另外一個座標系需要進行的轉動過程，由於座標系是兩兩相交的，於是只需要進行三次轉動變能得到新座標系，先討論單獨對Z軸進行轉動的情況。假設F=[r_x1 r_y1 r_z1 ]^T是以X1、Y1、Z1組成的座標系下的一個向量，現在座標系圍繞Z1轉動了α度,變成了 F_new=[r_x2 、r_y2 、r_z2 ]^T。如圖
三角形OBD和三角形DFE相似，所以角ADB等於α，DE=cosα*DF = cos r_y1,並且在三角形AOD中，AD=sinα*OD=sin r_x1求得的。上面三個公式組合起來便是繞Z旋轉的方向餘弦矩陣:
同理可得繞其他軸旋轉的方向餘弦矩陣。而能同時表示繞三軸旋轉的方向矩陣就是分別將繞單軸旋轉的矩陣相乘，矩陣的乘法是不可交換的，也就是說旋轉次序的不同也會導致矩陣的不同。這裏規定矩陣先繞Y軸旋轉β度，再繞X軸轉動α度，最後繞Z軸轉動γ度，得出由機體座標系到地球座標系的方餘弦矩陣
四元素
四元素是複數的多維度變化，正如複數可以表示平面座標系下的旋轉一樣，四元素能夠被用來表示空間直角座標系下的旋轉，他是由一個矢量和一個標量組成，標量表示旋轉的角度，矢量表示旋轉軸
爲了減少運算量以及避免萬向節死鎖的情況，融合算法採用了四元素運算，他和方向餘弦矩陣一樣能夠表示物體的旋轉情況，並且方向餘弦矩陣也有着與之對應的四元素矩陣，能通過四元素求得三個軸向的姿態角α、β、γ。
由於兩個矩陣等效，於是我們在整個計算過程中都可以使用四元素進行，而在最後需要獲取姿態角的時候，也就有四元素矩陣與方向餘弦矩陣的關係得到姿態角。對於四元數的更新可以使用一階龍格庫塔公式，傳入參數爲三軸角速度和取樣週期。公式的推導比較複雜，這裏不是關注重點也就不進行討論。最後推導出來。通過它也將姿態角連繫上了原始數據。
