4、姿态解算算法：互补滤波原理与实现、Mahony滤波算法、Madgwick滤波算法对比
姿态解算，说白了就是回答一个问题：“我的飞行器现在朝哪个方向？歪了多少？”
这个问题看似简单，但做起来坑不少。我刚开始做飞控那会儿，以为拿MPU6050读个加速度计和陀螺仪数据，直接算角度就行了。结果一上电，数据跳得像心电图，飞机还没离地，姿态角已经翻了几个跟头。
嗯，这里要注意：传感器都有各自的毛病。陀螺仪短期准，但长期漂移；加速度计长期稳，但短期噪声大。互补滤波，就是让它们俩“互补”一下。
核心思想：陀螺仪负责“快而准的短期变化”，加速度计负责“慢而稳的长期校正”。两者通过一个权重因子融合，得到平滑且无漂移的姿态。
互补滤波原理与实现
互补滤波的数学形式很简单。假设我们用四元数或欧拉角表示姿态，陀螺仪积分给出高频姿态变化，加速度计给出低频参考。
公式长这样：
姿态 = α × (陀螺仪积分结果) + (1 - α) × (加速度计/磁力计解算结果)其中α通常取0.98左右。α越大，越信任陀螺仪；α越小，越信任加速度计。
为什么是0.98？我习惯用这个经验值：陀螺仪占98%，加速度计占2%。这个比例在大多数四轴飞行器上表现不错。当然，如果你做的是剧烈运动的穿越机，可能需要把α调高到0.995。
我的经验：α的取值和你的运动剧烈程度直接相关。平稳飞行用0.98，剧烈机动用0.99以上。但别超过0.999，否则加速度计基本失效，姿态会慢慢漂走。
互补滤波的代码实现非常轻量。我曾在STM32F103上跑过，只占不到2KB的Flash和几十字节的RAM。对于资源受限的飞控来说，这是巨大的优势。
// 互补滤波简化实现（一阶）
void complementary_filter(float dt, float gx, float gy, float gz,
float ax, float ay, float az, float *roll, float *pitch) {
// 陀螺仪积分
float roll_gyro = *roll + gx * dt;
float pitch_gyro = *pitch + gy * dt;
// 加速度计解算
float roll_acc = atan2(ay, az);
float pitch_acc = atan2(-ax, sqrt(ay*ay + az*az));
// 互补融合
float alpha = 0.98f;
*roll = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
*pitch = alpha * pitch_gyro + (1.0f - alpha) * pitch_acc;
}这段代码看起来简单，但实际项目中要注意：加速度计数据必须先低通滤波。我曾经直接拿原始数据算角度，结果飞机悬停时姿态抖得像筛糠。后来加了个截止频率20Hz的IIR低通滤波器，世界瞬间安静了。
避坑指南：互补滤波在纯水平运动时表现良好，但如果你做的是带大角度倾斜的飞行器（比如固定翼做筋斗），加速度计会引入严重的离心力误差。这时候需要额外补偿。
Mahony滤波算法
Mahony滤波是互补滤波的“进阶版”。它用PI控制器来修正陀螺仪的偏差，而不是简单加权平均。
为什么需要PI控制器？你想想看，陀螺仪有零偏，积分久了就会漂。互补滤波只是把加速度计拉过来“拽”一下，但Mahony是主动去估计并补偿陀螺仪的零偏。
Mahony的核心步骤：
- 用当前姿态（四元数）预测重力方向
- 用加速度计测量实际重力方向
- 计算两者误差（叉积）
- 用PI控制器将误差反馈到陀螺仪角速度上
- 更新四元数
我记得第一次在Pixhawk上移植Mahony算法时，发现它的收敛速度比互补滤波快得多。飞机从任意初始姿态上电，不到1秒就能收敛到正确姿态。而互补滤波可能需要3-5秒。
// Mahony滤波核心（简化版）
void mahony_update(float dt, float gx, float gy, float gz,
float ax, float ay, float az) {
// 预测重力方向
float halfvx = q1*q3 - q0*q2;
float halfvy = q0*q1 + q2*q3;
float halfvz = q0*q0 - 0.5f + q3*q3;
// 计算误差（叉积）
float ex = ay * halfvz - az * halfvy;
float ey = az * halfvx - ax * halfvz;
float ez = ax * halfvy - ay * halfvx;
// 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;
// 四元数更新（一阶龙格库塔）
q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * dt;
q1 += ( q0*gx + q2*gz - q3*gy) * 0.5f * dt;
q2 += ( q0*gy - q1*gz + q3*gx) * 0.5f * dt;
q3 += ( q0*gz + q1*gy - q2*gx) * 0.5f * dt;
// 归一化
float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
}关键参数：Kp和Ki。Kp控制收敛速度，Ki控制零偏估计速度。我一般从Kp=0.5、Ki=0.001开始调。Kp太大，姿态会震荡；Ki太大，零偏估计会过冲。
Mahony的优点是计算量小、收敛快、能估计陀螺零偏。缺点是对加速度计噪声敏感，需要做好预处理。
Madgwick滤波算法
Madgwick算法是2010年由Sebastian Madgwick提出的。它用梯度下降法来优化姿态，而不是用PI控制器。
说白了，Mahony是“用误差修正角速度”，Madgwick是“用误差直接修正姿态”。
Madgwick的核心思想：
- 陀螺仪积分给出一个“预测姿态”
- 加速度计/磁力计给出一个“观测姿态”
- 用梯度下降法找到两者之间的最优姿态
我记得第一次看Madgwick的论文时，被里面的雅可比矩阵和梯度公式吓了一跳。但实际代码实现后，发现它的效果确实好——尤其是在有磁力计的情况下，能同时输出航向角。
// Madgwick滤波核心（简化版）
void madgwick_update(float dt, float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz) {
// 陀螺仪积分（四元数更新）
float qDot1 = 0.5f * (-q1*gx - q2*gy - q3*gz);
float qDot2 = 0.5f * ( q0*gx + q2*gz - q3*gy);
float qDot3 = 0.5f * ( q0*gy - q1*gz + q3*gx);
float qDot4 = 0.5f * ( q0*gz + q1*gy - q2*gx);
// 梯度下降法修正
float beta = 0.1f; // 梯度下降步长
// ...（此处省略雅可比矩阵和梯度计算，约50行代码）
// 实际项目中建议直接使用Madgwick开源代码
q0 += (qDot1 - beta * gradient1) * dt;
q1 += (qDot2 - beta * gradient2) * dt;
q2 += (qDot3 - beta * gradient3) * dt;
q3 += (qDot4 - beta * gradient4) * dt;
// 归一化
float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
}我的建议：Madgwick的代码量比Mahony大，但效果更好。如果你做的是带磁力计的飞控（比如航拍无人机），优先选Madgwick。如果只是纯姿态控制（比如穿越机），Mahony就够了。
三种算法对比
| 特性 | 互补滤波 | Mahony | Madgwick |
|---|---|---|---|
| 计算量 | 极低 | 低 | 中等 |
| 收敛速度 | 慢（3-5秒） | 快（<1秒） | 快（<1秒） |
| 陀螺零偏估计 | 无 | 有（PI控制） | 有（梯度下降） |
| 磁力计支持 | 需额外实现 | 需额外实现 | 原生支持 |
| 适用场景 | 玩具级、资源受限 | 穿越机、工业控制 | 航拍、导航级 |
| 代码量 | ~20行 | ~80行 | ~200行 |
避坑指南：我曾经在一个项目中用Madgwick算法，但磁力计没有做硬铁校准，结果航向角误差达到30度。后来加了椭球拟合校准，误差降到2度以内。记住：算法再好，也救不了烂数据。
如何选择？
我个人习惯这样选：
- 做玩具或低成本飞控（比如STM32F0、8位MCU）：用互补滤波，简单够用
- 做穿越机或竞速无人机：用Mahony，计算快、收敛快、零偏补偿好
- 做航拍无人机或导航级飞控：用Madgwick，精度高、支持磁力计
你想想看，如果你的飞控只有20MHz的主频，跑Madgwick可能CPU占用率就飙到80%了，这时候互补滤波反而是最佳选择。
嗯，最后说一句：没有最好的算法，只有最合适的算法。我在实际项目中，甚至见过把三种算法都实现，然后根据飞行模式动态切换的。比如起飞时用互补滤波（快速稳定），巡航时切到Madgwick（高精度）。
这就是姿态解算的“三剑客”。下一章我们聊聊卡尔曼滤波——那个让无数人又爱又恨的算法。