AHRS姿态估计

四元数

四元数最常见的定义 q 是以下形式的有序表达式：

四元数可以在几何空间中定义为旋转算子的替代形式，以便我们可以使用类似于其他三维欧几里得变换的乘积找到某个向量 a的图像 a′
四元数的结构可以用来描述一种轴-角表示的旋转，形式为：
这仅在四元数归一化时有效，在这种情况下它也被称为单位四元数。为了强制执行单位四元数，我们对四元数进行归一化：

四元数乘积使用汉密尔顿乘积进行乘法运算，它可以用向量形式表示，作为已知的标量-向量形式，甚至可以用矩阵乘法表示
四元数的共轭

方向余弦矩阵

方向余弦是向量与基坐标系之间夹角的余弦 [WikipediaDCM]。在这种情况下，方向余弦描述了正交向量ri与基坐标系之间的差异。包含这些差异的矩阵通常称为方向余弦矩阵。
这些矩阵主要用于两个目的：
描述任何框架相对于基准框架的方位。
将向量（表示三维空间中的点）从一个坐标系转换到另一个坐标系。这是一个旋转操作。由于后者，DCM 也被称为旋转矩阵。
因此，DCM 是旋转最常见的表示形式 [WikipediaRotMat]

Madgwick算法

Madgwick 滤波器由Sebastian Madgwick提出。它可以通过 3 轴加速度计、3 轴陀螺仪和 3 轴磁力计在短时间内准确计算物体的方向。该滤波器使用四元数作为方向表示，以描述 3D 世界中的物体方向，因四元数可以避免欧拉角的奇异性（也称为干涉锁）。Madgwick 滤波器的目标是最小化由原始陀螺仪数据产生的错误，这些数据总是包含高频噪声。因此，加速度计和磁力计用于计算陀螺仪测量误差，并通过梯度下降算法进行修正。
IMU 的配置
两种常用的导航坐标系配置：ENU 和 NED。与 ENU 对应的机体坐标系为右前上，与 NED 对应的机体坐标系为前右下。ENU 的旋转顺序为 y-x-z，NED 的旋转顺序为 x-y-z。横滚、俯仰和偏航分别对应于 X、Y 和 Z 轴。

角速度定向Orientation from angular rate

IMU 测量提供原始数据在其本体坐标系中，这可能与您期望的导航坐标系不对齐。请检查您的配置，如有必要，请处理坐标系转换。由于 Madgwick 滤波器的方向计算基于四元数，我们需要在一开始进行姿态初始化。姿态包括在欧拉角表示下的横滚、俯仰和偏航角。我们可以从加速度计数据计算横滚角和俯仰角，从磁力计数据计算偏航角。以下是两个连续测量之间的四元数微分方程及其定义。

梯度下降优化四元数的变化

由于我们的目标是最小化两个连续测量之间四元数变化的误差，我们可以定义以下等式，其中 dx、dy、dz 是导航坐标系中的测量值，sx、sy、sz 是机体坐标系中的传感器测量向量。在计算之前不要忘记对加速度计和磁力计读数进行归一化。

因此，目标函数是：

考虑到 6 轴和 9 轴传感器融合的情况，Madgwick 滤波器假设导航坐标系中的加速度计测量向量为[0, 0, g]。因此，目标函数及其雅可比矩阵如下所示。

对于磁力计测量向量，Madgwick 滤波器假设与磁东对齐的机体坐标轴是可以忽略的，这意味着在 ENU 坐标系中 X 轴为 0，在 NED 坐标系中 Y 轴为 0。磁力计向量在机体坐标系中定义为 hx、hy、hz，在导航坐标系中定义为 bx、by、bz。请记住，由于磁偏角和磁倾角的影响，磁力计测量的是 X、Y、Z 轴分量上的磁北，而不是磁正北。因此，我们需要从磁力计测量的 X、Y 轴计算磁北。

对于 9 轴传感器融合，将加速度计和磁力计读数在导航坐标系和机体坐标系中代入方程，可以得到目标函数及其雅可比矩阵，如下所示。

Madgwick 滤波器的梯度是目标函数与其雅可比矩阵的乘积，指的是由加速度计和磁力计校正的先验四元数的计算。此外，请别忘了对乘积进行归一化。

有一个 beta 参数，它指的是 Madgwick 滤波器的增益。你也可以将其视为 PID 控制器的 P 项。如果我们增加增益，这意味着更信任加速度计和磁力计，反之，减少增益则意味着更信任陀螺仪。增加增益的好处是滤波器对变化的响应更快，但这也会使滤波器对噪声变得敏感。如果我们决定降低增益，滤波器收敛所需的时间就会更长，这意味着在计算姿态时会有延迟。