Madgwick IMU Filter是一种用于六轴或九轴传感器的Orientation Filter，通过融合陀螺仪、加速度计和磁力计数据提供精确的姿态估算。它通过内部传感器积分和外部传感器优化的四元数方法，结合梯度下降法的改进策略，减少积分误差，实现高效融合。此滤波器特别适用于需要实时、高精度姿态数据的场景。
IMU 是指六轴传感器，包含陀螺仪和加速度计。MARG 是指九轴传感器，在 IMU 的基础上添加了磁力计。
IMU = gyroscope + accelerometer
MARG(Magnetic, Angular Rate, and Gravity) = gyroscope + accelerometer + magnetometer
为方便起见，下文中，我将 IMU 与 MARG 统称为 IMU。
原理
Madgwick 是一个 Orientation Filter，用于获得精确的姿态数据，并不考虑整个 IMU 的积分过程。
在 IMU 与其他传感器的融合中我们一般都把 IMU 当做是内部（Intrinsic）传感器。内部传感器能够获得精确的加速度、角速度数据，实际使用中将加速度进行二次积分、角速度进行一次积分获得位置与姿态。但是因为与外界缺乏联系，积分能迅速扩大误差的累积，所以内部传感器需要与外部传感器融合使用。
将 IMU 分解成陀螺仪、加速度计、磁力计，加速度计测量的是地球重力场（受力平衡情况下），磁力计测量的是地磁场（忽略人工磁场影响）。
所以在 IMU 受力平衡状态下，陀螺仪是内部传感器，加速度计和磁力计是外部传感器。
Madgwick IMU Update 是使用加速度计或磁力计的数据与陀螺仪融合使用，提供精确的姿态数据。
过程
Madgwick 使用两种方法分别使用内部传感器、外部传感器求出两个姿态（四元数），随后将这两个姿态进行融合。
1. 内部传感器积分的四元数
用陀螺仪测量值与 \(t-1\) 时刻的四元数积分计算出 \(t\) 时刻四元数。
陀螺仪的测量值：
\[\sideset{^S}{}\omega = \begin{bmatrix} 0 & \omega_x & \omega_y & \omega_z\end{bmatrix}\]
（注：左上角 \(S\) 表示在 Sensor 坐标系下的坐标。）
四元数关于时间 \(t\) 的一阶导数：
\[ \sideset{^S_E}{}{\dot{q}} = {1 \over 2} {}\sideset{^S_E}{}{\hat{q}} \otimes \sideset{^S}{}{\omega} \]
（注：左下角 \(E\) 表示 Earth 坐标系，\(\sideset{^S_E}{}{\dot{q}}\) 表示 Earth 坐标系在 Sensor 坐标系下的四元数时间变化率。）
代入时间，从 \(t-1\) 时刻计算到
最低0.47元/天 解锁文章
4219
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
