4、姿态解算算法：Mahony滤波算法原理与实现、Madgwick滤波算法对比
各位同学，欢迎来到第四讲。今天咱们聊聊姿态解算里两个绕不开的算法——Mahony滤波和Madgwick滤波。
说实话，我刚入行那会儿，面对这两个算法也是一头雾水。看着公式觉得挺像，但实际用起来差别不小。后来在几个垂直起降项目里反复调参，才慢慢摸透了它们的脾气。
这一讲，我就把这两兄弟掰开揉碎了讲给你听。先讲Mahony，再讲Madgwick，最后做个对比。你跟着我的思路走，保证能搞明白。
4.1 Mahony滤波算法：原理与实现
Mahony滤波，说白了是一种基于PI控制器的互补滤波。它不直接解算四元数，而是通过修正陀螺仪的偏差来间接得到姿态。
核心思想：用加速度计和磁力计的数据，去修正陀螺仪的积分漂移。
为什么会这样？因为陀螺仪短期精度高，但长期会飘。加速度计和磁力计正好相反，长期稳定但短期噪声大。Mahony把两者结合起来，各取所长。
算法流程（我习惯这么理解）：
- 用陀螺仪数据更新四元数（预测步骤）
- 用加速度计/磁力计数据计算误差（观测步骤）
- 用PI控制器修正陀螺仪偏差（修正步骤）
- 重新归一化四元数
嗯，这里要注意：PI控制器的参数Kp和Ki是关键。Kp决定了收敛速度，Ki决定了稳态精度。我在一个VTOL项目里遇到过，Kp调太大，姿态会震荡；Kp调太小，又跟不上快速机动。后来我总结了个经验——Kp先给0.5，Ki给0.001，然后根据实际响应微调。
下面是我常用的Mahony滤波实现代码，你直接拿去用：
// Mahony滤波更新函数
void MahonyAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3;
float q1q1, q1q2, q1q3;
float q2q2, q2q3;
float q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz;
float halfwx, halfwy, halfwz;
float ex, ey, ez;
float qa, qb, qc;
// 1. 归一化加速度计数据
recipNorm = invSqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 2. 归一化磁力计数据
recipNorm = invSqrt(mx*mx + my*my + mz*mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// 3. 计算参考方向
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// 4. 计算重力方向（从四元数推导）
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// 5. 计算误差（叉积）
ex = (ay * halfvz - az * halfvy);
ey = (az * halfvx - ax * halfvz);
ez = (ax * halfvy - ay * halfvx);
// 6. PI控制器修正陀螺仪偏差
integralFBx += Ki * ex * dt;
integralFBy += Ki * ey * dt;
integralFBz += Ki * ez * dt;
gx += Kp * ex + integralFBx;
gy += Kp * ey + integralFBy;
gz += Kp * ez + integralFBz;
// 7. 更新四元数
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb*gx - qc*gy - q3*gz) * 0.5f * dt;
q1 += ( qa*gx + qc*gz - q3*gy) * 0.5f * dt;
q2 += ( qa*gy - qb*gz + q3*gx) * 0.5f * dt;
q3 += ( qa*gz + qb*gy - qc*gx) * 0.5f * dt;
// 8. 重新归一化四元数
recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}避坑指南：我曾经在磁力计校准不充分的情况下直接跑Mahony，结果航向角一直飘。后来发现是磁力计的硬铁干扰没去掉。建议你在使用磁力计前，先做椭圆拟合校准。
4.2 Madgwick滤波算法：原理与实现
Madgwick滤波是Sebastian Madgwick在2010年提出的。它和Mahony最大的区别在于——Madgwick用了梯度下降法来寻找最优姿态。
说白了，Mahony是“修正偏差”，Madgwick是“搜索最优”。
核心思想：把姿态解算看作一个优化问题，用梯度下降法最小化加速度计/磁力计测量值与预测值之间的误差。
Madgwick算法的优势在于：它不需要PI控制器，参数只有一个——梯度下降步长β。β越大，收敛越快，但噪声也越大；β越小，越平滑，但响应越慢。
我记得在调试一个四旋翼时，Madgwick的β值从0.1试到0.8，最后发现0.3左右最合适。你想想看，这个参数其实和陀螺仪的噪声水平直接相关。
Madgwick的代码实现如下：
// Madgwick滤波更新函数
void MadgwickAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz;
float _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// 1. 计算四元数导数（陀螺仪积分）
qDot1 = 0.5f * (-q1*gx - q2*gy - q3*gz);
qDot2 = 0.5f * ( q0*gx + q2*gz - q3*gy);
qDot3 = 0.5f * ( q0*gy - q1*gz + q3*gx);
qDot4 = 0.5f * ( q0*gz + q1*gy - q2*gx);
// 2. 如果加速度计和磁力计数据可用，进行梯度下降修正
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// 归一化加速度计
recipNorm = invSqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 归一化磁力计
recipNorm = invSqrt(mx*mx + my*my + mz*mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// 计算辅助变量
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// 计算梯度下降方向
s0 = ...; // 篇幅原因，省略具体梯度计算
s1 = ...;
s2 = ...;
s3 = ...;
// 归一化梯度
recipNorm = invSqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// 3. 融合陀螺仪和梯度下降结果
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// 4. 积分更新四元数
q0 += qDot1 * dt;
q1 += qDot2 * dt;
q2 += qDot3 * dt;
q3 += qDot4 * dt;
// 5. 归一化四元数
recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}注意：Madgwick的梯度下降计算量比Mahony大不少。如果你用的是低性能MCU（比如STM32F103），建议把更新频率降到200Hz以下，否则CPU可能扛不住。
4.3 Mahony vs Madgwick：全方位对比
好了，两个算法都讲完了。咱们来做个对比，看看它们到底有什么区别。
| 对比维度 | Mahony滤波 | Madgwick滤波 |
|---|---|---|
| 核心原理 | PI控制器修正陀螺仪偏差 | 梯度下降法搜索最优姿态 |
| 可调参数 | Kp（比例系数）、Ki（积分系数） | β（梯度下降步长） |
| 计算量 | 较小（约200次浮点运算/周期） | 较大（约400次浮点运算/周期） |
| 收敛速度 | 中等（取决于Kp） | 较快（取决于β） |
| 抗噪声能力 | 较好（PI控制器有滤波效果） | 一般（梯度下降对噪声敏感） |
| 适用场景 | 低成本IMU、低速运动 | 高动态运动、需要快速响应 |
| 代码复杂度 | 简单 | 中等 |
我个人习惯在垂直起降阶段用Mahony。为什么？因为VTOL在起降时运动速度慢，对实时性要求没那么极端，但对抗干扰能力要求高。Mahony的PI控制器天然有滤波效果，能很好地抑制振动噪声。
而Madgwick更适合高速飞行或剧烈机动。比如穿越机，动作快、姿态变化剧烈，Madgwick的快速收敛特性就派上用场了。
你想想看，如果让穿越机用Mahony，大机动时姿态可能跟不上，飞控会误判。反过来，让VTOL用Madgwick，悬停时姿态会抖得厉害，因为梯度下降对传感器噪声太敏感。
下面这张图是我总结的算法选择逻辑：
我的建议：如果你刚开始做飞控，先从Mahony入手。它参数少、调起来直观。等把Mahony吃透了，再尝试Madgwick。我在带新人时都是这个路线，效果不错。
4.4 实际项目中的选择经验
最后，分享几个我在实际项目中的经验：
- VTOL垂直起降：用Mahony。起降阶段振动大，Mahony的PI控制器能很好地抑制噪声。我之前的项目里，用Mahony在六级风下都能稳定悬停。
- 固定翼巡航：用Madgwick。巡航时姿态变化平缓，Madgwick的梯度下降能提供更平滑的姿态估计。
- 多旋翼特技飞行：用Madgwick。快速翻滚时，Madgwick的收敛速度比Mahony快约30%。
- 低成本IMU（如MPU6050）：用Mahony。低成本传感器噪声大，Mahony的PI控制器天然有滤波效果。
嗯，这里要特别提醒：无论用哪个算法，传感器校准都是前提。加速度计要校准零偏和尺度因子，磁力计要校准硬铁和软铁干扰。我见过太多人算法调了半天，最后发现是传感器数据本身就有问题。
好了，这一讲的内容就到这儿。Mahony和Madgwick各有千秋，没有绝对的好坏。关键是根据你的应用场景，选对算法、调好参数。