mahony 算法是常見的姿態融合算法，將加速度計，磁力計，陀螺儀共九軸數據，融合解算出機體四元數，該算法可到其網站下載源碼https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
該篇僅介紹融合加速度計和陀螺儀的六軸數據算法，由於筆者水平有限，文中難免存在一些不足和錯誤之處，誠請各位批評指正。
1 空間姿態的常規描述
首先，姿態解算中的姿態實際上值得是機體坐標系與地理坐標系的旋轉關系。其常用描述形式有三種：歐拉角，方向余弦矩陣，四元數。
1.1 歐拉角與方向余弦矩陣
對於任何參考系，一個剛體的取向（也就是姿態解算中的姿態）可以圍繞剛體坐標系以z-x-z軸順序做三個歐拉角旋轉而得出的。另外的，剛體的取向可以用三個基本旋轉矩陣來確定，而將三個矩陣相乘可復合得到對於任何剛體旋轉的旋轉矩陣R，此處省略推導過程有興趣的話可以自行谷歌。
\[[\mathbf{R}]=\left[\begin{array}{ccc} {\cos \gamma} & {\sin \gamma} & {0} \\ {-\sin \gamma} & {\cos \gamma} & {0} \\ {0} & {0} & {1} \end{array}\right]\left[\begin{array}{ccc} {1} & {0} & {0} \\ {0} & {\cos \beta} & {\sin \beta} \\ {0} & {-\sin \beta} & {\cos \beta} \end{array}\right]\left[\begin{array}{ccc} {\cos \alpha} & {\sin \alpha} & {0} \\ {-\sin \alpha} & {\cos \alpha} & {0} \\ {0} & {0} & {1} \end{array}\right]\\ =\left[\begin{array}{ccc} {\cos \alpha \cos \gamma-\cos \beta \sin \alpha \sin \gamma} & {\sin \alpha \cos \gamma+\cos \beta \cos \alpha \sin \gamma} & {\sin \beta \sin \gamma} \\ {-\cos \alpha \sin \gamma-\cos \beta \sin \alpha \cos \gamma} & {-\sin \alpha \sin \gamma+\cos \beta \cos \alpha \cos \gamma} & {\sin \beta \cos \gamma} \\ {\sin \beta \sin \alpha} & {-\sin \beta \cos \alpha} & {\cos \beta} \end{array}\right]\\ \]
歐拉角描述空間姿態的一大優點就是方式直觀容易理解。但其存在的問題也很嚴重，即萬向節死鎖，網上有許多資料這里就不再贅述。
在經典歐拉角中我們圍繞剛體坐標軸以z-x-z軸順序旋轉進而推導出一種空間中坐標轉換矩陣，若圍繞地理坐標系中的坐標軸旋轉則可推導出以下繞三個坐標軸的旋轉矩陣，與歐拉角復合三個矩陣的方式相同，我們將三個矩陣相乘即可得到方向余弦矩陣C。
\[C_{1}^{}=\left[\begin{array}{ccc} {1} & {0} & {0} \\ {0} & {\cos \theta} & {\sin \theta} \\ {0} & {-\sin \theta} & {\cos \theta} \end{array}\right] \begin{array}{l} {C_{2}^{}=\left[\begin{array}{ccc} {\cos \gamma} & {0} & {-\sin \gamma} \\ {0} & {1} & {0} \\ {\sin \gamma} & {0} & {\cos \gamma} \end{array}\right]} \end{array} C_{3}^{}=\left[\begin{array}{ccc} {\cos \psi} & {-\sin \psi} & {0} \\ {\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1} \end{array}\right]\\ \]
\[C_{}^{}=C_1C_{2}C_{3}\\ C_{}^{}=\left[\begin{array}{ccc} {\cos \gamma \cos \psi+\sin \gamma \sin \psi \sin \theta} & {\sin \gamma \cos \psi \sin \theta-\cos \gamma \sin \psi} & {-\sin \gamma \cos \theta} \\ {\sin \psi \cos \theta} & {\cos \psi \cos \theta} & {\sin \theta} \\ {\sin \gamma \cos \psi-\cos \gamma \sin \psi \sin \theta} & {-\cos \gamma \cos \psi \sin \theta-\sin \gamma \sin \psi} & {\cos \gamma \cos \theta} \end{array}\right]\\ \]
上述表示方法雖然直觀，但其在公式中存在大量的三角函數式，而大量的三角函數會顯著延長運算時間導致計算周期變長。因此，我們需要另一種方便計算的描述方式——四元數。
1.2 四元數
四元數的定義與復數非常類似，唯一的區別是復數只有一個虛部，而四元數一共有三個。所有的四元數 q ∈ H
（H代表四元數的發現者William Rowan Hamilton）都可以寫成下面這種形式：
\[q=a+b i+c j+d k \quad(a, b, c, d \in \mathbb{R})\\ 或\\ q=q_{0}^{}+q_{1}^{} i+q_{2}^{} j+q_{3}^{} k \quad(q_{0}^{}, q_{1}^{}, q_{2}^{}, q_{3}^{} \in \mathbb{R})\\ \]
其中：
\[i^{2}=j^{2}=k^{2}=i j k=-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)\\ \]
1.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}\\ \]
1.2.1.2 加減運算與標量乘法
四元數的加減運算與標量乘法同復數類似，只需將分量各自運算即可，這里不做贅述。與復數相同，四元數的標量乘法同樣遵循交換律，即 \(sq = qs\)，其中s為標量。
1.2.1.3 四元數乘法
四元數乘法較為特殊，並不遵循交換律，也就是說在一般情況下\(q_1q_2 ≠ q_2q_1\)。
如果有兩個四元數 \(q_1 = a + bi + cj + dk\) 和 \(q_2 = e + fi + gj + hk\) ，由 \(i² = j² = k² = ijk = −1\) 可得其乘積為：
\[\begin{aligned} &q_{1} q_{2}=a e+a f i+a g j+a h k+\\ &b e i-b f+b g k-b h j+\\ &\begin{array}{l} {c e j-c f k-c g+c h i+} \\ {d e k+d f j-d g i-d h} \end{array}\\ &=(a e-b f-c g-d h)+\\ &(b e+a f-d g+c h) i\\ &(c e+d f+a g-b h) j\\ &(d e-c f+b g+a h) k \end{aligned}\\ \]
寫成矩陣形式則有：
\[q_{1} q_{2}=\left[\begin{array}{cccc} {a} & {-b} & {-c} & {-d} \\ {b} & {a} & {-d} & {c} \\ {c} & {d} & {a} & {-b} \\ {d} & {-c} & {b} & {a} \end{array}\right]\left[\begin{array}{l} {e} \\ {f} \\ {g} \\ {h} \end{array}\right]\\ \]
這個矩陣變換相當於左乘 \(q_1\)，由於四元數不符合交換律，所以右乘\(q_1\)的變換是一個不同的矩陣：
\[q_{2} q_{1}=\left[\begin{array}{cccc} {a} & {-b} & {-c} & {-d} \\ {b} & {a} & {d} & {-c} \\ {c} & {-d} & {a} & {b} \\ {d} & {c} & {-b} & {a} \end{array}\right]\left[\begin{array}{l} {e} \\ {f} \\ {g} \\ {h} \end{array}\right]\\ \]
1.2.1.4 純四元數
與復數相似，實部為0的四元數被稱之為純四元數，空間中的三維向量可以用純四元數的形式表示，這種表示方法在四元數表示旋轉的推導過程中有重要應用。
1.2.1.5 單位四元數
定義模長為1的四元數為單位四元數，用於表示旋轉的四元數一定為單位四元數。
2 利用四元數表示姿態
該部分省略推導過程，具體可以參考https://krasjet.github.io/quaternion/quaternion.pdf
2.1 利用四元數表示旋轉
通過羅德里格旋轉，我們可以推導出四元數表示旋轉的定理：
任意向量 v 繞着以單位向量定義的旋轉軸 u 旋轉 θ 度后的 v' 可以使用四元數乘法來獲得四元數旋轉公式:
\[v^{\prime}=q v q^{*}=q v q^{-1}\\ \]
其中：
\[v=[0, \mathbf{v}], \quad q=\left[\cos \left(\frac{1}{2} \theta\right), \sin \left(\frac{1}{2} \theta\right) \mathbf{u}\right]\\\\ \]
2.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\\ \]
矩陣的第一行和第一列不會對v進行任何變換，所以我們可以將其壓縮為3維方陣，即可得到矩陣旋轉公式：
任意向量 v 沿着以單位向量定義的旋轉軸 u 旋轉 θ 角度后的 v' 可以使用矩陣乘法來獲得：
\[\mathbf{v}^{\prime}=\left[\begin{array}{ccc} {1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \\ {2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} \\ {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \end{array}\right]\mathbf{v}\\ \]
同樣也是將向量從機體坐標系 b 到大地坐標系 R 的姿態矩陣 \(C_{b}^{R}\)（也稱為坐標轉換矩陣）即為：
\[C_{b}^{R}=\left[\begin{array}{lll} {1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \\ {2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} \\ {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \end{array}\right]\\ \]
這里再給出從大地坐標系 R 轉換到機體坐標系 b 的坐標轉換矩陣 \(C_{R}^{b}\)
\[C_{R}^{b}=\left[\begin{array}{lll} {1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} \\ {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} \\ {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \end{array}\right]\\ \]
其中：
\[q_{0}=\cos \left(\frac{1}{2} \theta\right), q_{1}=\sin \left(\frac{1}{2} \theta\right) u_{x}, q_{2}=\sin \left(\frac{1}{2} \theta\right) u_{y}, q_{3}=\sin \left(\frac{1}{2} \theta\right) u_{z}\\ \]
對應四元數為：
\[q=q_{0}^{}+q_{1}^{} i+q_{2}^{} j+q_{3}^{} k\\ \]
也可寫成有序數對形式，即2.1旋轉公式中的：
\[\quad q=\left[\cos \left(\frac{1}{2} \theta\right), \sin \left(\frac{1}{2} \theta\right) \mathbf{u}\right]\\ 其中\mathbf{u} = \left[\begin{array}{l} {u_{x}} \\ {u_{y}} \\ {u_{z}} \end{array}\right]\\ \]
2.3 通過四元數反解歐拉角
得到四元數后，可以通過四元數的值反解出機體坐標系的歐拉角，同樣的這里省略推導過程直接給出公式：
\[\left\{\begin{array}{c} {\theta=\arcsin [2(q_0 q_2-q_1 q_3)]} \\ {\gamma=\arctan \left(\frac{q_0 q_3+q_1 q_2}{1-2（q_2^{2}+q_3^{2}）}\right)} \\ {\psi=\arctan \left(\frac{q_0 q_1+q_2 q_3}{1-2（q_1^{2}+q_2^{2}）}\right)} \end{array}\right.\\ \]
3 基於四元數的姿態解算
3.1 四元數的求解
四元數描述機體坐標系與大地坐標系的旋轉關系，因此對於機體的姿態解算需要實時更新四元數。我們通過構建四元數關於時間的微分方程來研究此問題。
存在單位四元數：
\[Q=\cos \frac{\theta}{2}+\vec{u} \sin \frac{\theta}{2}\\ \]
對時間t進行微分，可得：
\[\frac{d Q}{d t}=\frac{1}{2}\left[\begin{array}{cccc} {0} & {-\omega_{x}} & {-\omega_{y}} & {-\omega_{z}} \\ {\omega_{x}} & {0} & {\omega_{z}} & {-\omega_{y}} \\ {\omega_{y}} & {-\omega_{z}} & {0} & {\omega_{x}} \\ {\omega_{z}} & {\omega_{y}} & {-\omega_{x}} & {0} \end{array}\right] \cdot\left[\begin{array}{c} {q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}} \end{array}\right]\\ \]
求解該微分方程即可得到當前四元數的值。但計算機中的計算是離散的，所以我們需要對該微分方程進行離散化處理，這樣才可以有效的通過單片機或其他數字控制器進行求解：
\[\left[\begin{array}{l} {q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}} \end{array}\right]_{t+\Delta t}=\left[\begin{array}{l} {q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}} \end{array}\right]_{t}+\frac{1}{2} \cdot \Delta t \cdot\left[\begin{array}{c} {-\omega_{x} \cdot q_{1}-\omega_{y} \cdot q_{2}-\omega_{z} \cdot q_{3}} \\ {\omega_{x} \cdot q_{0}-\omega_{y} \cdot q_{3}+\omega_{z} \cdot q_{2}} \\ {\omega_{x} \cdot q_{3}+\omega_{y} \cdot q_{0}-\omega_{z} \cdot q_{1}} \\ {-\omega_{x} \cdot q_{2}+\omega_{y} \cdot q_{1}+\omega_{z} \cdot q_{0}} \end{array}\right]\\ \]
3.2 傳感器數據融合
3.2.1 基本原理
首先，對於六軸數據，計算角度有兩種方法，一種是通過對角速度積分得到角度，另一種則是通過對加速度進行正交分解得到角度。但這兩種方式均存在不足，通過角速度積分得到角度時，角速度的誤差會在積分過程中被不斷放大從而影響數據准確性。而加速度計是一種特別敏感的傳感器，電機旋轉產生的震動會給加速度計的數據中帶來高頻噪聲。
不難看出，第一種方法測得的數據中存在結果偏差，而第二種方法測得的數據中存在高頻噪聲。因此可通過融合兩種數據以獲得准確姿態，這里通過加速度計補償角速度。
設有大地坐標下的重力加速度 g，把 g 通過姿態矩陣（坐標轉換矩陣）的逆（意味着從地理坐標系 R 到機體坐標 b 系）變換到機體坐標系，得到其在機體坐標系下的**理論重力加速度向量 \(\hat{\boldsymbol{v}}\) **，則兩者的變換關系可通過前文給出的姿態矩陣得出：
\[\hat{\boldsymbol{v}}=C_{R}^{b} \cdot \mathbf{g}=(C_{b}^{R})^{-1} \cdot\left[\begin{array}{l} {0} \\ {0} \\ {1} \end{array}\right]=\left[\begin{array}{c} {2\left(q_{1} q_{3}-q_{0} q_{2}\right)} \\ {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} \\ {1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \end{array}\right]=\left[\begin{array}{c} {v_{ x}} \\ {v_{ y}} \\ {v_{ z}} \end{array}\right]\\ \]
不難看出，將重力加速度向量變換至機體坐標系后，恰好是矩陣的最后一列。這樣一來，我們就得到了由描述剛體姿態的四元數推導出的理論重力加速度向量 \(\hat{\boldsymbol{v}}\) 。另外，我們還可以通過加速度計測量出實際重力加速度向量 \(\overline{\boldsymbol{v}}\) 。
顯然，這里的理論重力加速度向量 \(\hat{\boldsymbol{v}}\) 和實際重力加速度向量 \(\overline{\boldsymbol{v}}\) 之間必然存在偏差，而這個偏差很大程度上是由陀螺儀數據產生的角速度誤差引起的，所以根據理論向量和實際向量間的偏差，就可以補償陀螺儀數據的誤差，進而解算出較為准確的姿態，即將隱含在四元數中的角速度誤差顯化。
3.2.2 誤差補償
理論重力加速度向量和實際重力加速度向量均是向量，反應向量間夾角關系的運算有兩種：內積（點乘）和外積（叉乘），考慮到向量外積模的大小與向量夾角呈正相關，故通過計算外積來得到向量方向差值 \(θ\)：
\[|\mathbf{ρ}|=|\overline{\boldsymbol{v}}| \cdot|\hat{\boldsymbol{v}}| \cdot \sin \theta\\ \]
在進行叉乘運算前，應先將理論向量 \(\hat{\boldsymbol{v}}\) 和實際向量 \(\overline{\boldsymbol{v}}\) 做單位化處理，有：
\[|\mathbf{ρ}|=\sin \theta\\ \]
考慮到實際情況中理論向量 \(\hat{\boldsymbol{v}}\) 和實際向量 \(\overline{\boldsymbol{v}}\) 偏差角不會超過45°，而當\(θ\)在±45°內時，\(sinθ\) 與\(θ\)的值非常接近，因此上式可進一步簡化為：
\[|\mathbf{ρ}|=\theta\\ \]
得到向量偏差后，即可通過構建PI補償器來計算角速度補償值：
\[GyroError=K_{P} \cdot \text { error }+K_{I} \cdot \int \text {error}\\ \]
其中，比例項用於控制傳感器的“可信度”，積分項用於消除靜態誤差。\(K_P\)越大，意味着通過加速度計得到誤差后補償越顯著，即是越信任加速度計。反之\(K_P\)越小時，加速度計對陀螺儀的補償作用越弱，也就越信任陀螺儀。而積分項則用於消除角速度測量值中的有偏噪聲，故對於經過零篇矯正的角速度測量值，一般選取很小的\(K_I\)。最后將補償值補償給角速度測量值，帶入四元數差分方程中即可更新當前四元數。
3.3 回到歐拉角
考慮到四元數不具備直觀幾何意義，故最后還需通過四元數反解出歐拉角。這里直接套用上文給出的公式即可：
\[\left\{\begin{array}{c} {\theta=\arcsin [2(q_0 q_2-q_1 q_3)]} \\ {\gamma=\arctan \left(\frac{q_0 q_3+q_1 q_2}{1-2（q_2^{2}+q_3^{2}）}\right)} \\ {\psi=\arctan \left(\frac{q_0 q_1+q_2 q_3}{1-2（q_1^{2}+q_2^{2}）}\right)} \end{array}\right.\\ \]
3.4 代碼分析
//---------------------------------------------------------------------------------------------------
// Definitions
#define sampleFreq 1000.0f // sample frequency in Hz
#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
//---------------------------------------------------------------------------------------------------
// Variable definitions
volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef; // 2 * integral gain (Ki)
//volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
void MahonyAHRSupdateIMU(float q[4], float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
// 只在加速度計數據有效時才進行運算
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
// 將加速度計得到的實際重力加速度向量v單位化
recipNorm = invSqrt(ax * ax + ay * ay + az * az); //該函數返回平方根的倒數
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity
// 通過四元數得到理論重力加速度向量g
// 注意，這里實際上是矩陣第三列*1/2，在開頭對Kp Ki的宏定義均為2*增益
// 這樣處理目的是減少乘法運算量
halfvx = q[1] * q[3] - q[0] * q[2];
halfvy = q[0] * q[1] + q[2] * q[3];
halfvz = q[0] * q[0] - 0.5f + q[3] * q[3];
// Error is sum of cross product between estimated and measured direction of gravity
// 對實際重力加速度向量v與理論重力加速度向量g做外積
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// Compute and apply integral feedback if enabled
// 在PI補償器中積分項使能情況下計算並應用積分項
if(twoKi > 0.0f) {
// integral error scaled by Ki
// 積分過程
integralFBx += twoKi * halfex * (1.0f / sampleFreq);
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
// apply integral feedback
// 應用誤差補償中的積分項
gx += integralFBx;
gy += integralFBy;
gz += integralFBz;
}
else {
// prevent integral windup
// 避免為負值的Ki時積分異常飽和
integralFBx = 0.0f;
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
// 應用誤差補償中的比例項
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
// 微分方程迭代求解
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q[0];
qb = q[1];
qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz);
q[1] += (qa * gx + qc * gz - q[3] * gy);
q[2] += (qa * gy - qb * gz + q[3] * gx);
q[3] += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
// 單位化四元數 保證四元數在迭代過程中保持單位性質
recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;
//Mahony官方程序到此結束，使用時只需在函數外進行四元數反解歐拉角即可完成全部姿態解算過程
}
