前言

之前的一次推送介紹了Mahony姿態解算算法（IMU姿態濾波算法——Mahony算法：原理與代碼），這次介紹另一個經典的濾波算法：Madgwick濾波。

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

算法

2.1 重力方向對齊優化

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

假設world系中一個向量在world下的表示為 ，在傳感器下的觀測為 ，傳感器位姿以四元數表示記為 。如果此時的姿態是準確的，應該有：

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

令目標函數取到0（在四元數對應的參數空間上），則得到了最優的姿態，即。這可以當作一個優化問題進行求解。而優化問題的求解思路之一，就是采用梯度下降法。記的梯度為，那么在迭代優化求解時，下一次取值應該為當前取值減去當前梯度方向走一個步長，即

其中，

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

可以將步長 視作加速度計產生的數據對姿態的收斂速度，同時考慮到角速度在當前時刻會產生一個角速度的增量，因此加速度這部分的收斂必須大于角速度變化速率，即 ，其中 。

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

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

2.2 角速度融合

另一方面，我們可以通過角速度計提供的角速度積分，得到姿態，即：

這里我們對四元數進一步的區分。我們對上一時刻濾波后的姿態（四元數）記為 ，當前時刻角速度計的讀數即角速度為 ，角速度計計算得到的姿態為 。

對2.1中優化后得到的四元數記為 ，即加速度計計算得到的姿態。

那么，當前時刻估計的姿態取做兩個姿態的加權平均：

至于 如何取值，下一小節進行討論。

2.3 權重取值

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

即

由于 中對 沒有限制，因此取 為無限大，此時，綜合(1)(5)(6)，有：

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

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

代碼

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是利用優化；

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

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

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

編輯：黃飛