經(jīng)典的濾波算法：Madgwick濾波算法

前言

之前的一次推送介紹了Mahony姿態(tài)解算算法（IMU姿態(tài)濾波算法——Mahony算法：原理與代碼），這次介紹另一個(gè)經(jīng)典的濾波算法：Madgwick濾波。

Madgwick濾波算法根據加速度計、陀螺儀、以及磁力計，融合計算機體四元數，計算速度快、精度較高。本文詳細介紹六軸融合，即根據加速度計和陀螺儀數據，計算IMU的姿態(tài)。

算法

2.1 重力方向對齊優(yōu)化

首先要指出的是，Madgwick算法假設加速度計測量的加速度完全由重力提供，即物體本體運動(dòng)產(chǎn)生的加速度可忽略不計。這一點(diǎn)和Mahony算法是一樣的。

假設world系中一個(gè)向量在world下的表示為 ，在傳感器下的觀(guān)測為 ，傳感器位姿以四元數表示記為 。如果此時(shí)的姿態(tài)是準確的，應該有：

然而由于姿態(tài)不準，等式并不成立。因此，我們定義目標函數：

令目標函數取到0（在四元數對應的參數空間上），則得到了最優(yōu)的姿態(tài)，即。這可以當作一個(gè)優(yōu)化問(wèn)題進(jìn)行求解。而優(yōu)化問(wèn)題的求解思路之一，就是采用梯度下降法。記的梯度為，那么在迭代優(yōu)化求解時(shí)，下一次取值應該為當前取值減去當前梯度方向走一個(gè)步長(cháng)，即

其中，

其中， 為 的雅可比（具體的形式可參考文獻[1][2]）。

可以將步長(cháng) 視作加速度計產(chǎn)生的數據對姿態(tài)的收斂速度，同時(shí)考慮到角速度在當前時(shí)刻會(huì )產(chǎn)生一個(gè)角速度的增量，因此加速度這部分的收斂必須大于角速度變化速率，即 ，其中 。

對于一般的優(yōu)化，我們需要不斷迭代求解直到收斂，但Madgwick為了保證算法的實(shí)時(shí)性，只進(jìn)行一次梯度下降，也能取得差不多的精度。

具體地，如果我們把world系下的這個(gè)向量 取重力 ，姿態(tài)傳感器對應的測量 即為IMU的加速度計讀數 ，此時(shí)有：

2.2 角速度融合

另一方面，我們可以通過(guò)角速度計提供的角速度積分，得到姿態(tài)，即：

這里我們對四元數進(jìn)一步的區分。我們對上一時(shí)刻濾波后的姿態(tài)（四元數）記為 ，當前時(shí)刻角速度計的讀數即角速度為 ，角速度計計算得到的姿態(tài)為 。

對2.1中優(yōu)化后得到的四元數記為 ，即加速度計計算得到的姿態(tài)。

那么，當前時(shí)刻估計的姿態(tài)取做兩個(gè)姿態(tài)的加權平均：

至于 如何取值，下一小節進(jìn)行討論。

2.3 權重取值

由于角速度存在漂移，我們定義角速度的發(fā)散速率為 。這里如果加速度計對姿態(tài)貢獻的收斂速度，等于角速度的發(fā)散速度，則由式(5)融合的結果依舊是準確的姿態(tài)。即需要有：

即

由于 中對 沒(méi)有限制，因此取 為無(wú)限大，此時(shí)，綜合(1)(5)(6)，有：

定義估計的角速度 ，則最終有：

再看式(7)，可以將視作，下一時(shí)刻的姿態(tài)，等于上一時(shí)刻姿態(tài)，加上角速度計的積分，減去一個(gè)與角速度計噪聲水平 相關(guān)的增量，這個(gè)增量的與加速度計的優(yōu)化時(shí)的梯度有關(guān)。最終，Madgwick濾波算法的參數只有一個(gè)。

代碼

Matlab完整代碼[3]如下：

functionobj=UpdateIMU(obj,Gyroscope,Accelerometer)

q=obj.Quaternion;

if(norm(Accelerometer)==0),return;end
Accelerometer=Accelerometer/norm(Accelerometer);%歸一化加速度計數據

%式(2)和(3)
F=[2*(q(2)*q(4)-q(1)*q(3))-Accelerometer(1)
2*(q(1)*q(2)+q(3)*q(4))-Accelerometer(2)
2*(0.5-q(2)^2-q(3)^2)-Accelerometer(3)];
J=[-2*q(3),2*q(4),-2*q(1),2*q(2)
2*q(2),2*q(1),2*q(4),2*q(3)
0,-4*q(2),-4*q(3),0];
step=(J'*F);
step=step/norm(step);%式(1)中的減號后面的部分，即修正量

%式(8)中的修正角速度
qDot=0.5*quaternProd(q,[0Gyroscope(1)Gyroscope(2)Gyroscope(3)])-obj.Beta*step';

%式(7)(8)，即迭代到下一步。
q=q+qDot*obj.SamplePeriod;
obj.Quaternion=q/norm(q);%normalisequaternion
end

與Mahony算法的比較

Madgwick算法與Mahony算法相比，最大的不同之處是如何對待加速度計估計的誤差。Mahony是利用叉乘，Madgwick是利用優(yōu)化；

Mahony可以視作一個(gè)PI（比例-積分）控制器，Madgwick是一個(gè)P（比例）控制器；

Madgwick比Mahony的精度稍高一丟丟，但Mahony的計算速度略快[4]；

Mahony與Madgwick都需要假設加速度測的只是重力，因此在加速度變化劇烈情況下表現不佳。

編輯：黃飛