嵌入式常用滤波算法与控制算法（6）互补滤波

第 6 篇：互补滤波——陀螺仪+加速度计的最简融合方案

前面讲了卡尔曼滤波——数学严谨但调参麻烦。在姿态估计领域，有一种更简单实用的替代方案：互补滤波。代码量只有卡尔曼的 1/5，效果却不差。

1. 为什么需要"互补"？

陀螺仪和加速度计测量的是同一个角度，但噪声特征完全不同：

陀螺仪 (gyro)： ✅ 短时间非常准，不受振动影响 ❌ 积分会漂移——误差随时间累积（1 分钟漂 5°） 加速度计 (accel)： ✅ 长时间平均是准的（重力方向） ❌ 短时间噪声大——一振动就乱跳（±5°）

互补滤波的思路：

• 陀螺仪 → 可信高频部分（短时间内的角度变化）
• 加速度计 → 可信低频部分（长时间的角度基准）
• 用一高一低两个滤波器把它们"拼"起来

加速度计 → [低通] ─┐ ├→ 融合角度 陀螺仪 → [高通] ─┘

但实际上，高通 = 1 - 低通，所以只需要一个低通滤波器：

融合角度 = 低通(加速度计角度) + (1 - 低通)(陀螺仪积分角度)

2. 一行公式搞定

等价于这个递推公式：

angle = α × (angle + gyro × dt) + (1 - α) × accel_angle 或者更直观的写法： angle = (1 - α) × (angle + gyro × dt) + α × accel_angle └──── "陀螺仪占主导" ────┘ └── 加一点加速度计修正 ──┘

α 的含义：

• α 大 → 更信加速度计 → 收敛快但怕振动
• α 小 → 更信陀螺仪 → 平滑但怕漂移
• 典型值：α = 0.02（2% 加速度计 + 98% 陀螺仪）

3. C 代码实现（不到 30 行）

// comp_filter.htypedefstruct{floatangle;// 当前角度估计floatalpha;// 互补系数 (0.0 ~ 1.0)floatdt;// 采样周期（秒）}comp_filter_t;voidcomp_init(comp_filter_t*cf,floatalpha,floatdt);floatcomp_update(comp_filter_t*cf,floataccel_angle,floatgyro_rate);

// comp_filter.c#include"comp_filter.h"voidcomp_init(comp_filter_t*cf,floatalpha,floatdt){cf->alpha=alpha;cf->dt=dt;cf->angle=0;}floatcomp_update(comp_filter_t*cf,floataccel_angle,floatgyro_rate){// 陀螺仪积分 → 预测下一秒的角度floatgyro_prediction=cf->angle+gyro_rate*cf->dt;// 互补融合：大部分信陀螺仪，小部分拉回加速度计cf->angle=(1.0f-cf->alpha)*gyro_prediction+cf->alpha*accel_angle;returncf->angle;}

4. 实战：MPU6050 全姿态（Roll + Pitch）

comp_filter_troll_cf,pitch_cf;voidimu_init(void){mpu6050_init();comp_init(&roll_cf,0.02f,0.005f);// α=0.02, dt=5mscomp_init(&pitch_cf,0.02f,0.005f);}voidimu_read(float*roll,float*pitch){floatax,ay,az,gx,gy,gz;mpu6050_read_all(&ax,&ay,&az,&gx,&gy,&gz);// 加速度计推算角度floataccel_roll=atan2f(ay,az);floataccel_pitch=atan2f(-ax,sqrtf(ay*ay+az*az));// 陀螺仪速率（rad/s）直接传给互补滤波*roll=comp_update(&roll_cf,accel_roll,gx);*pitch=comp_update(&pitch_cf,accel_pitch,gy);}

α 值调参实验：

5. 互补滤波 vs 卡尔曼滤波

选择建议：

• 做平衡车、云台、航模 →互补滤波（够用且简单）
• 做无人机导航、机器人定位 →卡尔曼（精度要求高）

下一篇：限幅滤波 + 消抖滤波——两种轻量级方案，各 10 行代码