本文使用 Zhihu On VSCode 創作併發布

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]\\

尤拉角描述空間姿態的一大優點就是方式直觀容易理解。但其存在的問題也很嚴重，即萬向節死鎖，網上有許多資料這裡就不再贅述。上述表示方法雖然直觀，但其在公式中存在大量的三角函式式，而大量的三角函式會顯著延長運算時間導致計算週期變長。因此，我們需要另一種方便計算的描述方式——四元數。

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}<br>{x} \\<br>{y} \\<br>{z}<br>\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/quate

rnion/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

的

姿態矩陣 #FormatImgID_23#

（也稱為

座標轉換矩陣

）即為：

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官方程式到此結束，使用時只需在函式外進行四元數反解尤拉角即可完成全部姿態解算過程

}