第四节：互补滤波融合——从原理到实战调优

互补滤波，这个名字听起来挺学术的。但说白了，它就是解决一个很实际的问题：陀螺仪短期准，但长期飘；加速度计短期抖，但长期稳。怎么把这两者的优点结合起来？互补滤波就是干这个的。

我在项目里用过不少滤波方案，卡尔曼当然好，但很多时候——尤其是资源受限的嵌入式平台——互补滤波反而是更务实的选择。它计算量小，调参直观，而且效果并不差。

4.1 互补滤波的核心思想

先想一个问题：你要估计一个物体的姿态角。陀螺仪给出角速度积分，短时间内很准，但积分误差会随时间累积。加速度计给出重力方向参考，没有累积误差，但高频噪声大。

互补滤波的思路很简单：让陀螺仪通过高通滤波器，让加速度计通过低通滤波器，然后把它们加起来。这样，短期靠陀螺仪，长期靠加速度计，各取所长。

核心公式（一阶形式）：

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

其中 α 是滤波系数，通常取 0.98 ~ 0.995 之间。

α 越大，越信任陀螺仪，响应快但可能漂移；α 越小，越信任加速度计，更稳定但响应慢。这个权衡，就是调参的核心。

4.2 一阶互补滤波实现

一阶互补滤波是最基础的版本。我最早在STM32F103上跑过，代码非常简洁。

// 一阶互补滤波
float complementary_filter_1st(float angle_acc, float gyro_rate, float dt, float alpha) {
    static float angle = 0.0f;
    angle = alpha * (angle + gyro_rate * dt) + (1.0f - alpha) * angle_acc;
    return angle;
}

这段代码看起来简单，但有几个坑要注意：

初始值问题：angle 的初始值最好用加速度计的第一帧数据，否则起步会有跳变。
dt 必须准确：我见过有人用固定 dt，但实际循环时间会波动，导致积分误差。建议用定时器测量真实 dt。
alpha 的选取：一般经验是，如果传感器更新率 100Hz，alpha 取 0.98 左右。更新率越高，alpha 可以越接近 1。

我的经验： 曾经在一个四轴飞行器项目里，我直接把 alpha 设成 0.99，结果悬停时角度慢慢漂了。后来发现是 dt 没更新，导致积分步长不对。修好 dt 后，问题就解决了。

4.3 二阶互补滤波

一阶滤波有个问题：它对加速度计的噪声抑制不够干净，而且相位滞后比较明显。二阶互补滤波通过引入一个积分项，能更好地平滑加速度计信号，同时保持对陀螺仪的快速响应。

二阶滤波的公式稍微复杂一点：

// 二阶互补滤波
typedef struct {
    float angle;
    float bias;  // 陀螺仪零偏估计
} Complementary2nd_t;

void complementary_filter_2nd(Complementary2nd_t *state, 
                              float angle_acc, float gyro_rate, 
                              float dt, float kp, float ki) {
    float error = angle_acc - state->angle;
    state->bias += ki * error * dt;
    state->angle += (gyro_rate - state->bias + kp * error) * dt;
}

这里 kp 和 ki 相当于 PID 控制器的比例和积分系数。kp 控制对加速度计的信任程度，ki 用来消除陀螺仪的零偏漂移。

注意： 二阶滤波的 ki 不能太大，否则会引入低频振荡。我一般从 kp=0.1, ki=0.001 开始调，然后根据响应曲线微调。

4.4 自适应互补滤波

固定系数的互补滤波有个硬伤：运动剧烈时，加速度计不可信，应该更多依赖陀螺仪；运动平稳时，加速度计可信，应该更多依赖它。自适应互补滤波就是根据运动状态动态调整 α 或 kp/ki。

怎么判断运动状态？一个常用的方法是计算加速度计的模值：

float acc_norm = sqrt(ax*ax + ay*ay + az*az);
float alpha_adaptive = alpha_base;

// 如果加速度模值偏离重力加速度太多，说明运动剧烈
if (fabs(acc_norm - 9.81f) > 0.5f) {
    alpha_adaptive = 0.995f;  // 更信任陀螺仪
} else {
    alpha_adaptive = 0.98f;   // 正常信任加速度计
}

我在一个手持设备项目里用过这个方案。用户快速挥动设备时，加速度计数据乱跳，但自适应滤波能自动切换到陀螺仪主导，姿态估计非常稳。

自适应策略总结：

加速度模值接近 1g：运动平稳，增大加速度计权重
加速度模值偏离 1g：存在线加速度，减小加速度计权重
角速度变化剧烈：可能快速旋转，适当降低陀螺仪权重（防止过冲）

4.5 三种滤波方案的对比

方案	计算量	抗噪声能力	动态响应	适用场景
一阶互补滤波	极低	一般	中等	简单姿态估计，资源受限
二阶互补滤波	低	较好	快	需要零偏补偿的场景
自适应互补滤波	中等	好	自适应	运动状态变化大的场景

我个人习惯是：如果 MCU 资源紧张，用一阶；如果对零偏敏感，用二阶；如果运动模式复杂，用自适应。没有绝对的好坏，只有合不合适的方案。

4.6 核心知识体系

下面这张图是我梳理的互补滤波知识结构，方便你快速把握整体脉络：

4.7 调参实战建议

调互补滤波，我总结了一个三步法：

先固定 dt：确保采样时间稳定，这是所有滤波的基础。
粗调 α 或 kp：让系统在静态时能收敛到加速度计参考值，动态时能跟上陀螺仪。
细调 ki 或自适应阈值：消除稳态误差，优化动态切换的平滑度。

一个小技巧： 调参时可以用串口把角度、加速度计角度、陀螺仪积分三个值同时打印出来，画成曲线看。我习惯用匿名上位机或者 SerialPlot，一眼就能看出滤波效果好不好。

嗯，关于互补滤波，核心就是这些。从一阶到二阶再到自适应，每一步都是在解决前一步的局限性。你想想看，其实很多工程问题都是这样——没有银弹，只有不断权衡和优化。

可可精品资料库

📚 课程目录

第四节：互补滤波融合——从原理到实战调优

4.1 互补滤波的核心思想

4.2 一阶互补滤波实现

4.3 二阶互补滤波

4.4 自适应互补滤波

4.5 三种滤波方案的对比

4.6 核心知识体系

4.7 调参实战建议