经典的滤波算法：Madgwick滤波算法
前言
之前的一次推送介绍了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]如下：
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都需要假设加速度测的只是重力，因此在加速度变化剧烈情况下表现不佳。
地震预警基准站的智能防雷，防雷工程解决方案
什么是保险丝？保险丝的种类、工作原理以及选用标准
IGBT漏电流增加有哪些原因
iPhone贴膜加壳实属多余：装饰作用大过保护
汽车控制器气密性检测，直压气密性检测设备技术
经典的滤波算法：Madgwick滤波算法
Vivado IDE全面了解XDC文件的约束顺序
开关电源中的＂电流互感器＂电路如何定参数？
闪迪推出全新嵌入式闪存驱动器，供给中国的OEM，容量高达64GB
放大电路中耦合电容和旁路电容如何判别？
重磅！亚马逊要求卖家3月7日前须执行这项认证，否则产品将面临移除风险
2020打响第一枪 百度AI体系开始进行组织架构升级
解决方案 | 3D视觉引导胶管无序抓取上下料
新品 | 联讯仪器 PXIe 精密源表S2013C，高速高精度测量！
为高音质而研制，应用于英飞凌EVAL_MA5302MS参考设计中的数字功放电感
加速国产 扎根开源丨中软国际多款国产应用产品精彩亮相
光伏发电智能诊断系统的研究
可解释人工智能（XAI）在军事领域的作用
轰轰烈烈国际化，中国家电各大巨头开始崛起
无人机为我国大气治理提供重要支撑
之前的一次推送介绍了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]如下：
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都需要假设加速度测的只是重力，因此在加速度变化剧烈情况下表现不佳。
地震预警基准站的智能防雷，防雷工程解决方案
什么是保险丝？保险丝的种类、工作原理以及选用标准
IGBT漏电流增加有哪些原因
iPhone贴膜加壳实属多余：装饰作用大过保护
汽车控制器气密性检测，直压气密性检测设备技术
经典的滤波算法：Madgwick滤波算法
Vivado IDE全面了解XDC文件的约束顺序
开关电源中的＂电流互感器＂电路如何定参数？
闪迪推出全新嵌入式闪存驱动器，供给中国的OEM，容量高达64GB
放大电路中耦合电容和旁路电容如何判别？
重磅！亚马逊要求卖家3月7日前须执行这项认证，否则产品将面临移除风险
2020打响第一枪 百度AI体系开始进行组织架构升级
解决方案 | 3D视觉引导胶管无序抓取上下料
新品 | 联讯仪器 PXIe 精密源表S2013C，高速高精度测量！
为高音质而研制，应用于英飞凌EVAL_MA5302MS参考设计中的数字功放电感
加速国产 扎根开源丨中软国际多款国产应用产品精彩亮相
光伏发电智能诊断系统的研究
可解释人工智能（XAI）在军事领域的作用
轰轰烈烈国际化，中国家电各大巨头开始崛起
无人机为我国大气治理提供重要支撑