1.姿态的描述----坐标系
為了建立運動物體運動姿态的數學模型，首先需要定義導航坐标系和載體坐标系。
導航坐标系n（O-X -Y -Z ）選取東-北-天建立坐标系，坐标原點為 O點，Y 軸正向指向北邊，X 軸正向指向東邊，
Z 軸正向指向天；載體坐标系 b（O-X -Y -Z ）選取右手直角坐标系，即向右為 X 軸正方向，向前為 Y 軸正方向，
向上為 Z 軸正方向。
2.四元數基礎理論
3.互補濾波器原理
根據傳感器之間的頻域的互補特性，準備采用互補濾波算法對傳感器的資料進行姿态角的求解。
陀螺儀動态響應特性好，但是在計算姿态時候，會有積分累計誤差；而加速度計和磁力計不會有累計誤差，但是他們動态響應特性差。
是以，三個傳感器在頻域上互補，故可以采用互補濾波器對傳感器資料進行融合，進行姿态解算。
利用互補濾波器進行姿态資訊資料融合中，将由加速度計和磁力計估算出的姿态資訊通過低通濾波器與陀螺積分出的姿态進行進行融合。通過低通濾波器可以去除加速度信号和磁力計信号中夾雜的高頻信号，與陀螺信号積分的姿态進行進行互補後可有效抑制陀螺漂移。
4.基于互補濾波姿态解算過程
具體過程如下：
(1)姿态估計初始，需要對四元數進行指派
上面給出了，基于歐拉角對四元數的表示，具體的推導過程後面的文章會給出來，今天隻講一下整體的思路。
根據該公式，我們就可以确定四元數的初值。一般情況，理想狀态下，我們認為剛開始歐拉角為0，是以，得到的此時的四元數為[1 0 0 0 ].當然這個初值也友善後面的計算。
(2) 資料預處理
為了消除原始噪聲的幹擾，進一步提高載體姿态估計精度。
在進行互補濾波姿态解算之前，對傳感器原始資料進行濾波處理。将加速度計測得的載體坐标系下的加速度原始資料 A 經過 Butterworth 低通濾器後進行标準化，得到載體坐标
系下三軸加速度的分量夠成的向量 為：
實際上，加速度計的校正隻是把b 系和 n系的 XOY 平面重合起來，是以隻能校
正俯仰角和橫滾角，無法感覺載體繞 Z 軸的旋轉運動，即偏航角仍然存在角度誤差，
是以需要磁力計來進一步進行校正。校正過程如下：
将磁力計測得資料進行歸一化：
将磁力計資料從載體系轉換到導航系，磁力計在導航系下的輸出為
其中，mx,my,mz分别是磁力計傳感器的實際測量值。
定義導航系中 X 軸指向正北方向。根據磁場原理，導航系下的 Z 軸方向分量相
等，且水準方向分量相等，由此得到磁力計在導航系下的理想輸出為：
式中，bx,by,bz分别為磁力計在導航系下的理想輸出在三個軸上的分量。
将導航下的磁力計的理想輸出資料’轉換到載體坐标系下，可得：
根據同一個向量在不同的坐标系下，向量的大小和方向理論上是一樣的，我們将在載體坐标系下，把磁力計傳感器的實際測量值和轉換到載體坐标系下的磁力計的理想輸出值，做叉乘運算，也叫向量積運算。
原因：
根據向量積公式：
在這裡θ表示a和b之間的角度（0°≤θ≤180°），它位于這兩個矢量所定義的平面上。而n是一個與a、b所在平面均垂直的機關矢量。
**對叉乘後的等式兩邊同時取模，我們就得到叉乘的結果是兩個向量的夾角的正弦值，而在這個角度很小的時候，我們可以認為這個正弦值近似等于這個夾角。
**
是以說，叉乘後的結果近似看出兩個向量的夾角了，那麼，這個夾角有啥含義呢？
我們知道，加速度傳感器可以得到橫滾角和俯仰角，而磁力計傳感器可以得到偏航角，那麼，不難知道了，把加速度計和磁力計的理論輸出和實際測量值，做個叉乘運算後，這個角度正是橫滾角，俯仰角和偏航角的角度的誤差！
下面分别是磁力計，加速度計的理論值和實際值的叉乘運算：
這樣我們就得到了我們的誤差，下面把這個誤差求和後，加給陀螺儀傳感器輸出的角速度上，作為新的角速度值，通過四元數微分方程，進而求得四元數。
下圖是新的角速度：
（因為陀螺儀随着時間會有累計誤差，是以需要修正陀螺儀的誤差)
對于這個誤差，我們采用PID控制器或者PI控制器進行調節，
根據補償後的角速度資料，帶入四元數微分方程，求解四元數。
求解出來的四元數如下所示：
其中，T為傳感器采集資料的周期。
最後一步，把四元數轉換為歐拉角：
說明：歐拉角的旋轉順序有12種，常用的順序是Z-Y-X。這個自己來确定，不同的旋轉順序對應的四元數和歐拉角的轉換公式也不同，這個我還需要再看看。
好，今天先寫到這裡，整個流程就結束了。
後面會推到一些細節公式。