第四章 传感器数据融合：卡尔曼滤波入门、互补滤波、Mahony滤波在姿态解算中的应用

各位同学，欢迎来到第四章。前面我们聊完了传感器特性，也讲了怎么给原始数据做预处理。但说实话，单个传感器总有它的脾气——加速度计怕震动，陀螺仪会漂移，磁力计容易受干扰。你想想看，要是只信其中一个，飞控的姿态解算迟早要翻车。

所以这一章，我们专门来解决一个问题：怎么把多个传感器的数据揉在一起，取长补短，得到一个靠谱的姿态角？ 这就是传感器数据融合的核心。

我个人习惯把姿态融合算法分成三类：互补滤波、Mahony滤波、卡尔曼滤波。别被名字吓到，说白了它们都是在做同一件事——用陀螺仪的短时精度，去修正加速度计和磁力计的长时漂移。只是实现方式不同，适用场景也不同。

4.1 互补滤波：最简单实用的入门方案

互补滤波，名字听着高大上，其实原理特别朴素。它假设陀螺仪的高频信号是准的，加速度计的低频信号是准的。那好办，把两者通过一个低通和一个高通滤波器组合起来就行。

公式长这样：

角度 = α × (角度 + 陀螺仪角速度 × dt) + (1 - α) × 加速度计角度

这里的 α 就是那个“信任比例”，一般在 0.9 到 0.99 之间。α 越大，越信任陀螺仪；α 越小，越信任加速度计。

我在项目中遇到过一个坑：有次调试四旋翼，发现悬停时姿态一直在低频晃动。查了半天，发现是 α 设得太大了（0.995），导致加速度计的修正作用太弱，陀螺仪的微小漂移慢慢累积，最后变成了可见的晃动。把 α 降到 0.98，问题立刻解决。

互补滤波的优点是计算量极小，在早期的 8 位单片机上都能跑得飞起。缺点也很明显——它只能处理一维的角度融合（比如横滚或俯仰单独算），对于三维姿态的耦合问题，它有点力不从心。

4.2 Mahony滤波：四元数版本的互补滤波

Mahony 滤波，你可以把它理解为“用四元数实现的互补滤波”。它解决了互补滤波只能处理一维角度的问题，直接在三维空间里做姿态融合。

它的核心思路是：用加速度计和磁力计的测量值，计算出一个“误差修正量”，然后用这个修正量去补偿陀螺仪的漂移。

伪代码大概长这样：

// 1. 用陀螺仪数据更新四元数
q = q + 0.5 * q * ω * dt

// 2. 用加速度计计算误差
a_measured = 归一化后的加速度计读数
a_estimated = 从四元数估计出的重力方向
error = cross(a_measured, a_estimated)

// 3. 用误差修正陀螺仪
ω = ω + Kp * error + Ki * ∫error dt

// 4. 重复步骤1

这里 Kp 和 Ki 是两个关键参数。Kp 决定了修正的力度，Ki 负责消除稳态误差。

我曾经在调试一款飞翼布局的固定翼时，发现 Mahony 滤波在大机动转弯后，姿态恢复得特别慢。后来分析发现是 Ki 设得太小了，积分项来不及把陀螺仪的零偏误差吃掉。把 Ki 从 0.001 调到 0.005，恢复速度明显改善。

Mahony 滤波的优势是计算量适中，在 STM32F4 级别的芯片上可以跑到 1kHz 以上。而且它天然支持四元数输出，方便后续的控制律计算。

4.3 卡尔曼滤波入门：从“猜”到“信”

卡尔曼滤波，很多人一听就头大。其实你把它当成一个“会学习的滤波器”就好理解了。

它的核心思想是：我不确定当前的状态，但我有两个信息来源——一个是模型预测（陀螺仪积分），一个是观测（加速度计/磁力计）。我根据两者的不确定性，动态地决定更相信谁。

卡尔曼滤波的五个核心公式，我建议你死记硬背下来：

步骤	公式	通俗解释
1. 状态预测	x̂ = A·x + B·u	用陀螺仪数据，猜下一步姿态
2. 协方差预测	P̂ = A·P·Aᵀ + Q	这个猜测有多不靠谱？
3. 卡尔曼增益	K = P̂·Hᵀ·(H·P̂·Hᵀ + R)⁻¹	更信模型还是更信观测？
4. 状态更新	x = x̂ + K·(z - H·x̂)	用观测值修正猜测
5. 协方差更新	P = (I - K·H)·P̂	修正后，不确定性降低了多少？

这里面有两个关键参数：Q（过程噪声协方差） 和 R（观测噪声协方差）。Q 越大，说明你越不相信陀螺仪；R 越大，说明你越不相信加速度计。

我在项目中遇到过一个经典问题：有次把卡尔曼滤波用在固定翼上，发现起飞后姿态角一直在缓慢漂移。检查了半天，发现是 Q 矩阵设得太小了，导致滤波器过于相信陀螺仪，忽略了加速度计的修正。把 Q 的对角线元素从 0.001 调到 0.01，漂移问题立刻消失。

4.4 三种滤波的对比与选型建议

说了这么多，到底该用哪个？我根据自己的经验，整理了一个对比表：

算法	计算量	精度	调参难度	适用场景
互补滤波	极低	一般	简单	低成本 MCU、简单姿态解算
Mahony 滤波	低	良好	中等	大多数固定翼、多旋翼
卡尔曼滤波	高	优秀	复杂	高精度要求、传感器噪声大

我个人习惯是：原型验证阶段用 Mahony 滤波，因为它参数少、调起来快。等飞控稳定了，再根据实际需求决定要不要换成卡尔曼滤波。

说实话，对于大多数固定翼无人机，Mahony 滤波的精度已经足够了。卡尔曼滤波虽然理论上更优，但它的性能提升往往被传感器本身的噪声和延迟所限制。你想想看，如果加速度计本身就有 0.5° 的噪声，卡尔曼滤波再厉害，也不可能把精度推到 0.1°。

4.5 实战中的避坑指南

最后，分享几个我在实际项目中踩过的坑：

• 传感器时间同步问题： 我曾经遇到过陀螺仪和加速度计的数据不是同一时刻采集的，导致融合出来的姿态在高频震动时出现“相位差”。解决办法是在代码里加上时间戳对齐，或者用插值法把数据对齐到同一时刻。
• 磁力计干扰： 有次在铁质平台上调试，磁力计读数被严重干扰，导致 Mahony 滤波的航向角直接偏了 30°。后来加了硬铁和软铁校准，才把问题解决。嗯，这里要注意，磁力计一定要做现场校准。
• 初始化姿态： 卡尔曼滤波在启动时需要一段收敛时间。我建议在飞控上电后，先保持无人机静止 2~3 秒，让滤波器完成初始化。否则一上来就剧烈运动，滤波器很容易发散。

好了，这一章的内容就到这里。下一章我们会进入更深入的领域——如何用这些融合后的姿态数据，设计出真正能飞的增稳控制律。到时候我们会聊到 PID 控制、角速度环和姿态环的配合，以及那些让你头疼的调参问题。

记住，传感器融合是飞控的“眼睛”，眼睛不好，后面的一切都是白搭。多花点时间把这一章吃透，后面的路会顺很多。