IMU姿态滤波算法——Madgwick算法：原理与代码

前言

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

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

算法

2.1 重力方向对齐优化

首先要指出的是，Madgwick算法假设加速度计测量的加速度完全由重力提供，即物体本体运动产生的加速度可忽略不计。这一点和Mahony算法是一样的。

假设world系中一个向量在world下的表示为 ，在传感器下的观测为 ，传感器位姿以四元数表示记为 。如果此时的姿态是准确的，应该有：

然而由于姿态不准，等式并不成立。因此，我们定义目标函数：

令目标函数取到0（在四元数对应的参数空间上），则得到了最