前言

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

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

算法

2.1 重力方向?qū)R優(yōu)化

2.2 角速度融合

2.3 權(quán)重取值

代碼

Matlab完整代碼[3]如下：

function?obj?=?UpdateIMU(obj,?Gyroscope,?Accelerometer)???
q?=?obj.Quaternion;???
if(norm(Accelerometer)?==?0),?return;?end????
Accelerometer?=?Accelerometer?/?norm(Accelerometer);?%?歸一化加速度計數(shù)據(jù)????
??%?式(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,?[0?Gyroscope(1)?Gyroscope(2)?Gyroscope(3)])?-?obj.Beta?*?step';??
%?式(7)(8)，即迭代到下一步。????q?=?q?+?qDot?*?obj.SamplePeriod;??
obj.Quaternion?=?q?/?norm(q);?%?normalise?quaternion
end

與Mahony算法的比較

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

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

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

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

參考資料

[1] Madgwick算法提出的report：An ecient orientation lter for inertial and inertial/magnetic sensor arrays （https://x-io.co.uk/downloads/madgwick_internal_report.pdf）

[2] Madgwick算法在ICRR會議的論文：Estimation of IMU and MARG orientation using a gradient descent algorithm

[3] Madgwick本人實現(xiàn)的Matlab/C/C#代碼：https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

[4] Comparison of Attitude and Heading Reference Systems using Foot Mounted MIMU Sensor Data: Basic, Madgwick and Mahony