前言

【资料图】

之前的一次推送介绍了Mahony姿态解算算法（IMU姿态滤波算法——Mahony算法：原理与代码），这次介绍另一个经典的滤波算法：Madgwick滤波。

Madgwick滤波算法根据加速度计、陀螺仪、以及磁力计，融合计算机体四元数，计算速度快、精度较高。本文详细介绍六轴融合，即根据加速度计和陀螺仪数据，计算IMU的姿态。

算法

2.1 重力方向对齐优化

2.2 角速度融合

2.3 权重取值

代码

Matlab完整代码[3]如下：

function obj = 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, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]) - obj.Beta * step';  % 式(7)(8)，即迭代到下一步。    q = q + qDot * obj.SamplePeriod;  obj.Quaternion = q / norm(q); % normalise quaternionend

与Mahony算法的比较

Madgwick算法与Mahony算法相比，最大的不同之处是如何对待加速度计估计的误差。Mahony是利用叉乘，Madgwick是利用优化；

Mahony可以视作一个PI（比例-积分）控制器，Madgwick是一个P（比例）控制器；

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

Mahony与Madgwick都需要假设加速度测的只是重力，因此在加速度变化剧烈情况下表现不佳。

参考资料

[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本人实现的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