1. 說在前面的

之前調(diào)試基于QMI8658 6軸姿態(tài)解算的時候，我對Mahony濾波的認(rèn)識還比較淺薄。初次的學(xué)習(xí)和代碼的移植讓我對四元數(shù)、歐拉角、旋轉(zhuǎn)余弦矩陣有了一定的接觸。然而，直到我將地磁傳感器加入到我的硬件，進行9軸姿態(tài)解算時，我深深地感受到對四元數(shù)與姿態(tài)陣之間關(guān)系的理解是非常重要的。在此之前，我檢索了網(wǎng)絡(luò)文章，并記錄了《MEMS_慣性傳感器09 - Mahony姿態(tài)解算算法詳解》、《MEMS_慣性傳感器14 - Mahony濾波算法的代碼分析》兩篇文章?？赡軐τ诖罄小①Y深工程師來說，這些文章的嚴(yán)謹(jǐn)性不夠，可能會誤導(dǎo)別人。但我必須承認(rèn)，我仍然沒有扎實的數(shù)學(xué)計算研究背后更深層次的理論。即使如此，我依然希望通過自己的努力在慣性導(dǎo)航算法上學(xué)習(xí)得更深入一些。同時，我也想多記錄一些學(xué)到的知識，多記錄一些自己的理解和思考。希望這些記錄能幫助到一些初學(xué)者。如果我的文章有錯誤的引導(dǎo)，我非常希望大佬們能夠指正，因為這對我來說是最大的收獲，我非常樂意傾聽和學(xué)習(xí)。我愿意自我突圍，向著理想前進!

這次學(xué)習(xí)檢索時發(fā)現(xiàn)了一個博文內(nèi)容硬核、有理有據(jù)的博主:路癡導(dǎo)航員[1]。她的博文 《AHRS互補濾波(Mahony)算法及開源代碼》對我?guī)椭艽蟆?/p>

2.Mahony濾波算法的核心思想

Mahony濾波算法的核心思想是通過將角速度和角度的測量值與歷史估計值進行加權(quán)平均，從而得到更加準(zhǔn)確的姿態(tài)估計結(jié)果。同時，濾波算法還利用加速度計的測量值來糾正姿態(tài)角度的漂移。該算法也存在一些缺點，例如對于快速旋轉(zhuǎn)和加速度變化較大的情況，其姿態(tài)估計結(jié)果可能會出現(xiàn)較大的誤差。

3. 易懂的理解 Mahony 濾波算法的過程

為了更好的理解，表達式先不用矩陣的思想，在算法的實現(xiàn)的過程中，需要將表達式轉(zhuǎn)換成矩陣形式。

① 根據(jù)陀螺儀的測量數(shù)據(jù)計算出角速度的增量，即：

Δθ = 0.5 * (gyro + bias) * Δt

其中，gyro表示陀螺儀的測量值，bias表示陀螺儀的零偏誤差，Δt表示時間間隔。

② 根據(jù)加速度計的測量數(shù)據(jù)計算出重力方向的估計值，即：

g_est = q * (0, 0, -1) * q^-1

其中，q表示四元數(shù)的姿態(tài)估計值。

③ 根據(jù)加速度計的測量數(shù)據(jù)和重力方向的估計值，計算出加速度計的誤差，即：

acc_err = acc_meas - g_est

其中，acc_meas表示加速度計的測量值。

④ 根據(jù)角速度的增量和加速度計的誤差，更新四元數(shù)的姿態(tài)估計值，即：

q = q * exp(Δθ - K * acc_err)

其中，K為控制增益。

4. 其他的一些思考

傳感器EVB板的測量數(shù)據(jù)以載體坐標(biāo)系(b-框架)表示。歐拉角的直觀表示描述了地理坐標(biāo)系中的角度，其中包括重力的影響。地理坐標(biāo)系中的重力矢量為[0,0,1]g，磁傳感器將地球磁場中的強度表示為[cos(θ),0,sin(θ)]guess。因此，在算法中，加速度和磁數(shù)據(jù)被歸一化，確保它們的模是一定大小的。

四元數(shù)從地理坐標(biāo)系(東北天)轉(zhuǎn)換到載體坐標(biāo)系，就得到了旋轉(zhuǎn)矩陣Cnb。

Cnb的最后一列取負(fù)，即將其轉(zhuǎn)換為載體坐標(biāo)系中的值。在理想情況下，如果沒有誤差，這個矢量應(yīng)該與測量值相等。然而，由于這兩者總會存在差異，因此我們需要利用它們之間的差值來修正陀螺儀更新的不準(zhǔn)確性。

在修正的過程中，我們認(rèn)為加速度計測量的可信度較高，還需要使用地磁傳感來修正加速度傳感的Z軸。磁力計的模的長度是確定的，但是兩個分量的具體大小無法像重力加速度一樣確定。因此，我們使用上一次的四元數(shù)得到的Cbn(從載體到地理坐標(biāo)系的旋轉(zhuǎn)矩陣)來計算測量的磁場強度。理論上，此刻的Cbn應(yīng)該在東向上的分量是零[cos(θ),0,sin(θ)]，而北向和地向會有分量。然而，由于此刻的Cbn尚未得到，我們使用的是上一次的Cbn，并加上一些測量誤差，導(dǎo)致東向上仍會有分量存在。為了處理這種情況，我們將xy平面的分量合成到一個方向上，使得y軸上的分量為0。這樣，處理后的地磁信息的地位就與重力加速度(0，0，-1)相似了。

根據(jù)余弦矩陣和歐拉角的定義，我們可以將地理坐標(biāo)系的重力向量轉(zhuǎn)換到載體坐標(biāo)系中。具體而言，將四元數(shù)轉(zhuǎn)換成方向余弦矩陣后，我們可以得到方向余弦矩陣的第三列的三個元素，即vx、vy和vz。這三個元素實際上表示了當(dāng)前歐拉角(即四元數(shù))在載體坐標(biāo)系上換算得到的重力單位向量。

axayaz是載體坐標(biāo)參照系上，加速度計測出來的重力向量。vxvyvz是陀螺積分后的姿態(tài)推算出的重力向量。它們之間的誤差向量exeyez是陀螺積分后的姿態(tài)和加計測出來的姿態(tài)之間的誤差。誤差向量可以用向量叉積來表示。叉積向量exeyez位于載體坐標(biāo)系上，且其大小與陀螺積分誤差成正比，可用于糾正陀螺。由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現(xiàn)在對載體坐標(biāo)系的糾正。

矢量之間的叉乘公式為err = A × B = |A| × |B| × sin(β)。當(dāng)兩個矢量重合時，誤差為零，表示沒有差異。當(dāng)兩個矢量之間夾角為90°時，誤差達到最大值，表示差異最大。然而，在夾角為90°附近的區(qū)間內(nèi)，誤差的變化是非線性的，這可能導(dǎo)致一些準(zhǔn)確性問題。

審核編輯：湯梓紅