1. 說在前面的

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

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

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

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

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

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

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

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

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

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

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

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

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

acc_err = acc_meas - g_est

其中，acc_meas表示加速度計(jì)的測量值。

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

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

其中，K為控制增益。

4. 其他的一些思考

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

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

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

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

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

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

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

審核編輯：湯梓紅