4、传感器融合：加速度计、陀螺仪、磁力计，谁更可信？
做飞控调参这些年，我见过不少新手一上来就盯着PID猛调，结果飞机还是晃得跟喝醉了似的。其实很多时候，问题根本不在控制，而在——传感器没整明白。
你想想看，飞控大脑接收到的姿态数据，全靠加速度计、陀螺仪、磁力计这三个家伙提供。它们各有各的脾气，各有各的毛病。谁在什么时候更可信？怎么把它们揉到一起？这就是传感器融合要干的事。
核心观点：没有绝对可信的传感器，只有动态可信的融合策略。低频信任加速度计和磁力计，高频信任陀螺仪。
4.1 三员大将，各怀绝技
先说说这三个传感器各自的看家本领和致命短板。
| 传感器 | 测量对象 | 强项 | 致命短板 |
|---|---|---|---|
| 陀螺仪 | 角速度 | 高频响应快，动态性能好 | 零偏漂移，积分后角度误差随时间累积 |
| 加速度计 | 比力（重力+运动加速度） | 长期稳定，不漂移 | 对振动敏感，无法区分重力与运动加速度 |
| 磁力计 | 地磁场矢量 | 提供绝对航向参考 | 极易受电机电流、铁磁物质干扰 |
说白了，陀螺仪是个短跑冠军，爆发力强但跑偏了也不知道。加速度计和磁力计是长跑选手，方向感好但步子慢，还容易受外界干扰。
4.2 为什么不能只用陀螺仪？
我刚开始做飞控时，也想过一个偷懒的办法：只用陀螺仪积分出角度不就行了？
结果你猜怎么着？飞机悬停10秒钟，姿态就偏了5度。原因很简单——陀螺仪有零偏。哪怕静止时输出不为零，积分一秒就是一个小误差，一分钟下来，飞机已经以为自己倒立了。
举个例子：
// 假设陀螺仪静止时输出 0.1°/s 的零偏
// 积分 60 秒后的角度误差：
float error = 0.1 * 60; // = 6°
// 飞控以为自己在倾斜，实际上水平
嗯，这就是为什么必须引入加速度计和磁力计来做长期校正。
4.3 互补滤波：最简单的融合思路
互补滤波的思路很直观：
- 陀螺仪负责短时间内的快速姿态变化（高通）
- 加速度计+磁力计负责长时间内的姿态基准（低通）
公式长这样：
角度 = 0.98 × (上一时刻角度 + 陀螺仪角速度 × dt) + 0.02 × 加速度计角度
这里的0.98和0.02就是权重系数。系数怎么定？
我个人习惯的做法是：先根据陀螺仪噪声带宽估算截止频率。比如陀螺仪有效信号在10Hz以上，加速度计噪声主要在5Hz以下，那截止频率设在2~3Hz就挺合适。
调参小技巧：互补滤波的截止频率 f_c = (1 - α) / (2π × dt × α)。α越接近1，陀螺仪权重越大，响应快但易漂移；α越小，加速度计校正越强，但会引入延迟。
4.4 卡尔曼滤波：更聪明的融合
互补滤波虽然简单，但它有个问题——权重是固定的。不管飞机是悬停还是剧烈翻滚，都用同一套系数。
卡尔曼滤波就不一样了。它会根据当前的运动状态，动态调整对每个传感器的信任程度。
我曾经在调试一款测绘无人机时遇到过这种情况：飞机在做大机动转弯，加速度计测到的比力里混入了大量运动加速度。如果用互补滤波，姿态会瞬间被带偏。但卡尔曼滤波能通过协方差矩阵自动降低加速度计的权重，全靠陀螺仪撑住姿态。
核心流程就五步：
- 预测：用陀螺仪数据推算下一时刻姿态
- 更新协方差：估算预测的不确定度
- 计算卡尔曼增益：根据测量噪声和预测噪声动态调整
- 校正：用加速度计/磁力计测量值修正预测值
- 更新协方差：为下一轮迭代做准备
代码骨架大概是这样：
// 预测步骤
x = F * x + B * u; // 状态预测
P = F * P * F' + Q; // 协方差预测
// 校正步骤
K = P * H' * inv(H * P * H' + R); // 卡尔曼增益
x = x + K * (z - H * x); // 状态更新
P = (I - K * H) * P; // 协方差更新
注意：卡尔曼滤波的调参难点在于Q和R矩阵的设置。Q太大，滤波响应快但噪声大；R太大，滤波平滑但延迟高。我建议先从单位矩阵开始，再根据实际飞行数据微调。
4.5 磁力计的特殊处理
磁力计是个让人头疼的家伙。它提供的航向信息虽然绝对，但干扰源太多了——电机电流、电源线、甚至附近的水泥钢筋都能让它跑偏。
我踩过的坑是：有一次在室内调试，磁力计读数一直稳定，我以为没问题。结果一飞到室外，航向直接偏了30度。后来才发现，室内地板里有钢筋网。
所以磁力计的使用要遵循几个原则：
- 必须做硬铁和软铁校准，消除固定偏差和比例偏差
- 只在低动态时信任磁力计，大机动时降低其权重
- 配合GPS航向，双冗余校验
4.6 融合策略对比
| 融合方法 | 计算量 | 动态性能 | 长期稳定性 | 适用场景 |
|---|---|---|---|---|
| 互补滤波 | 极低 | 中等 | 中等 | 入门、玩具级飞控 |
| 卡尔曼滤波 | 较高 | 优秀 | 优秀 | 专业飞控、测绘无人机 |
| Mahony/Madgwick | 低 | 良好 | 良好 | 开源飞控（PX4/ArduPilot） |
我个人更推荐在工程实践中使用Mahony滤波。它本质上是互补滤波的非线性版本，计算量比卡尔曼小得多，但效果在大多数场景下已经足够好。
4.7 传感器融合的完整流程
下面这张图展示了传感器融合的核心逻辑：
4.8 实战中的避坑指南
最后分享几个我踩过的坑，希望能帮你少走弯路：
- 加速度计不要直接当角度用：我曾经犯过这个错，以为atan2(ax, az)就是俯仰角。结果飞机一加速，姿态就乱跳。记住，加速度计测的是比力，不是重力。
- 磁力计校准不能省：有一次我偷懒没做硬铁校准，结果航向误差在10度到30度之间随机跳。后来老老实实做了8字校准，问题才解决。
- 融合频率要匹配：陀螺仪更新率如果是1kHz，加速度计只有100Hz，那融合循环要以最低频率为准，否则会出现数据不同步。
- 初始化时不要信任任何传感器：上电瞬间，陀螺仪零偏还没稳定，加速度计可能有冲击。我习惯等2秒后再开始融合。
一句话总结：传感器融合的本质，就是用陀螺仪的短时精度，去填补加速度计和磁力计的动态缺陷；再用加速度计和磁力计的长时稳定性，去修正陀螺仪的漂移。三者互补，才能得到可靠的姿态。