零.先修知識補充
0.1機體座標系
機體座標系，有名體軸系，是指固定在飛行器或者飛機上的遵循右手法則的三維正交直角座標系。
在機體座標系下，飛機本身是靜止的。然而，在飛行中，飛機的機體座標系與大氣流座標系之間存在相對運動。因此需要引入一個相對於機體座標系固定的參考點，來描述飛機在飛行過程中相對於大氣流的運動狀態。在飛機的機體座標系下，可以定義三個座標軸：X軸指向飛機的前進方向，Y軸指向飛機的右側方向，Z軸指向飛機的下方方向
因此，在機體座標系下，可以測量出相對於飛機前進方向的速度和相對於飛機旋轉軸的旋轉角速度。機體座標系下的速度被稱為機體速度，它描述了飛機相對於自身的運動狀態，包括飛機相對於空氣的速度以及空氣對飛機的作用力。機體座標系下的機身旋轉角速度被稱為機體角速度，它描述了飛機相對於自身旋轉的速度，包括繞X、Y、Z三個軸的旋轉速度。機體角速度是描述飛機姿態和穩定性的重要參數，能夠幫助飛行員控制飛機的飛行姿態
0.2慣性座標系
座標系的指向在慣性空間中保持不變的座標系稱為慣性座標系。慣性系與慣性座標系是有區別的。慣性系是慣性參考系的簡稱。滿足牛頓第一定律的參考系稱為慣性參考系。且相對慣性系作勻速直線運動的物體也是慣性參考系
慣性座標系是為了簡化世界座標系到物體座標系的轉化而產生的。慣性座標系的原點與物體座標系的原點重合，慣性座標系的軸平行於世界座標系的軸。引入了慣性座標系之後，物體座標系轉換到慣性座標系只需旋轉，從慣性座標系轉換到世界座標系只需平移。例如，現在有一個骰子，原地旋轉之後，希望可以描述它的旋轉，向左轉多少或者向右轉多少。這樣就要給定一個原始狀態，在這個狀態給定下，我們就可以輕松的描述它是如何旋轉的，那麽這個原始狀態本質上就是慣性座標系，只不過這個慣性座標系原點與骰子質心一致，兩個座標軸分別平行於標準座標系
0.3大地座標系
大地座標系是大地測量中以參考橢球面為基準面建立起來的座標系。地面點的位置用大地經度L、大地緯度B和大地高度H表示。大地座標系的確立包括選擇一個橢球、對橢球進行定位和確定大地起算數據。 一個形狀、大小和定位、定向都已確定的地球橢球叫參考橢球。參考橢球一旦確定，則標誌著大地座標系已經建立
一.Mahony演算法是什麽？
Mahony演算法是常見的姿態融合演算法，將加速度計、磁力計、陀螺儀共九軸數據，融合解算出機體四元數。本篇文章只介紹融合加速度計和陀螺儀的六軸數據演算法
二.空間姿態的常規描述
姿態解算中的姿態實際上指的是機體座標軸與地理座標系的旋轉關系。其常用描述形式有三種：歐拉角、方向余弦矩陣，四元數
2.1四元數
四元數與定義與復數類似，但復數只有一個虛部，而四元數一共有三個。所有四元數都可以寫成下面形式：
q=a+bi+cj+dk （ a,b,c,d\in R ）或 q=q_0+q_1i+q_2j+q_3k ( q_0,q_1,q_2,q_3\in R )
/q表示四元數/
/ i^2=j^2=k^2=ijk=-1 /
與復數類似，四元數就是基{1,i,j,k}的線性組合。同樣的，四元數也可以寫成向量形式：
q=\left[\begin{array}{l}{q_{0}^{}} \\{q_{1}^{}} \\{q_{2}^{}} \\{q_{3}^{}}\end{array}\right]\\
另外，我們也可以分離實部與虛部，即透過一個三維向量表示虛部，從而將四元數表示為純量與向量的有序對形式
q=[s, \mathbf{v}] \quad\left(\mathbf{v}=\left[\begin{array}{l}{x} \\{y} \\{z}\end{array}\right], s, x, y, z \in \mathbb{R}\right)\\
2.1.1 模長
仿照復數的定義，我們可以將四元數 q=a+bi+cj+dk 的模長定義為
||q||=\sqrt{a^2+b^2+c^2+d^2}
用有序形式表示為
\begin{aligned}\|q\| &=\sqrt{s^{2}+\|\mathbf{v}\|^{2}} \\&=\sqrt{s^{2}+\mathbf{v} \cdot \mathbf{v}}\end{aligned}\\
2.1.2加減運算與純量乘法
四元數的加減運算與純量乘法同復數類似，將分量各自運算即可
與復數相同，四元數的純量乘法同樣遵循交換律，即sq = qs ，其中s為純量
2.1.3四元數乘法
不遵循交換律，即一般情況下 q_1q_2\ne q_2q_1
證明：
2.1.4純四元數
與復數類似，實部為0的四元數被稱為純四元數
2.1.5單位四元數
定義模長為1的四元數為單位四元數
/用於表示旋轉的四元數一定是單位四元數！/
三.利用四元數表示姿態
3.1利用四元數表示旋轉
3.1.1羅德裏格旋轉公式
3.1.2四元數旋轉公式
透過羅德裏格旋轉公式，我們可以推匯出四元數表示旋轉的定理：
3.2利用四元數表示姿態矩陣
將四元數旋轉公式表示為矩陣形式：
q v q^{*}=\left[\begin{array}{cccc} {1} & {0} & {0} & {0} \\ {0} & {1-2 q_{2}^{2}-2 q_{3}^{2}} & {2 q_{1} q_{2}-2 q_{0} q_{3}} & {2 q_{0} q_{2}+2 q_{1} q_{3}} \\ {0} & {2 q_{1} q_{2}+2 q_{0} q_{3}} & {1-2 q_{1}^{2}-2 q_{3}^{2}} & {2 q_{2} q_{3}-2 q_{0} q_{1}} \\ {0} & {2 q_{1} q_{3}-2 q_{0} q_{2}} & {2 q_{0} q_{1}+2 q_{2} q_{3}} & {1-2 q_{1}^{2}-2 q_{2}^{2}} \end{array}\right] v\\
壓縮為三維方陣，得到矩陣旋轉公式：
3.3歐拉角
3.3.1歐拉角演算法
為了使四元數更具有操作性，更好理解，我們引入歐拉角
實作旋轉方式：
3.3.2透過四元數反解歐拉角
四.基於四元數的姿態解算
4.1四元數求解
四元數描述機體座標系與大地座標系的旋轉關系，因此對於機體的姿態解算需要即時更新四元數。透過構建四元數關於時間的微分方程式來研究此問題
4.2傳感器數據融合
4.2.1基本原理
對於六軸數據，計算角速度的方法有兩種，一是透過對角速度積分得到角度，另一種則是透過對加速度進行正交分解得到角度
但是這兩種方法各有不足：
因此，可以透過融合兩種數據以獲得準確姿態，即透過加速度計補償角速度
4.2.2誤差補償
