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

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

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

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

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

scss 复制代码

陀螺仪 (gyro)：
✅ 短时间非常准，不受振动影响
❌ 积分会漂移------误差随时间累积（1 分钟漂 5°）

加速度计 (accel)：
✅ 长时间平均是准的（重力方向）
❌ 短时间噪声大------一振动就乱跳（±5°）

互补滤波的思路：

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

css 复制代码

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

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

scss 复制代码

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

2. 一行公式搞定

等价于这个递推公式：

matlab 复制代码

angle = α × (angle + gyro × dt) + (1 - α) × accel_angle

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

α 的含义：

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

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

c 复制代码

// comp_filter.h
typedef struct {
    float angle;    // 当前角度估计
    float alpha;    // 互补系数 (0.0 ~ 1.0)
    float dt;       // 采样周期（秒）
} comp_filter_t;

void  comp_init(comp_filter_t *cf, float alpha, float dt);
float comp_update(comp_filter_t *cf, float accel_angle, float gyro_rate);

c 复制代码

// comp_filter.c
#include "comp_filter.h"

void comp_init(comp_filter_t *cf, float alpha, float dt) {
    cf->alpha = alpha;
    cf->dt    = dt;
    cf->angle = 0;
}

float comp_update(comp_filter_t *cf, float accel_angle, float gyro_rate) {
    // 陀螺仪积分 → 预测下一秒的角度
    float gyro_prediction = cf->angle + gyro_rate * cf->dt;

    // 互补融合：大部分信陀螺仪，小部分拉回加速度计
    cf->angle = (1.0f - cf->alpha) * gyro_prediction
              + cf->alpha * accel_angle;

    return cf->angle;
}

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

c 复制代码

comp_filter_t roll_cf, pitch_cf;

void imu_init(void) {
    mpu6050_init();
    comp_init(&roll_cf,  0.02f, 0.005f);  // α=0.02, dt=5ms
    comp_init(&pitch_cf, 0.02f, 0.005f);
}

void imu_read(float *roll, float *pitch) {
    float ax, ay, az, gx, gy, gz;
    mpu6050_read_all(&ax, &ay, &az, &gx, &gy, &gz);

    // 加速度计推算角度
    float accel_roll  = atan2f(ay, az);
    float accel_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);
}

α 值调参实验：

α 值	抗振动	抗漂移	收敛速度	推荐
0.005	极好	差（漂移明显）	慢	飞行器悬停
0.02	好	好	中	推荐默认
0.05	中	好	快	快速运动
0.1	差	好	很快	仅初始对准时用

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

	互补滤波	卡尔曼滤波
代码量	15 行	80 行
调参	1 个参数 α	2 个矩阵 Q, R
CPU 开销	极小	中等（矩阵运算）
精度	够用（±1°）	更高（±0.5°）
理论最优	❌ 不是	✅ 是（线性高斯假设下）

选择建议：

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

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