为什么需要滤波

传感器原始数据充满噪声：

  • MPU6050 加速度计高频抖动 → 角度漂移
  • 超声波 测距偶尔跳变 → 距离突跳
  • ADC 采样噪声 → 电压值不稳

选对滤波算法，效果天差地别。

一、滑动平均滤波

最简单、最常用。取 N 次采样的平均值。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/* 滑动平均滤波器 */
#define WINDOW_SIZE  5

typedef struct {
  float buf[WINDOW_SIZE];
  uint8_t index;
  float sum;
  uint8_t filled;
} MovingAvg;

float MovingAvg_Update(MovingAvg *f, float x) {
  if (f->filled < WINDOW_SIZE) {
    f->filled++;
    f->sum += x;
    f->buf[f->index] = x;
    f->index = (f->index + 1) % WINDOW_SIZE;
    return f->sum / f->filled;
  }
  f->sum -= f->buf[f->index];
  f->buf[f->index] = x;
  f->sum += x;
  f->index = (f->index + 1) % WINDOW_SIZE;
  return f->sum / WINDOW_SIZE;
}

特点：

优点 缺点
实现极简单 有 N/2 样本延迟
有效抑制随机噪声 对脉冲干扰敏感
计算量小 窗口越大响应越慢

适用： ADC 采样平滑、超声波测距、温度读数

二、中值滤波

取 N 次采样的中间值，对脉冲干扰免疫。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
/* 中值滤波 */
#define MEDIAN_SIZE  5

float Median_Update(float x) {
  static float buf[MEDIAN_SIZE];
  static uint8_t index = 0;
  float tmp[MEDIAN_SIZE];

  buf[index] = x;
  index = (index + 1) % MEDIAN_SIZE;

  /* 复制并排序 */
  memcpy(tmp, buf, sizeof(buf));
  for (int i = 0; i < MEDIAN_SIZE - 1; i++) {
    for (int j = 0; j < MEDIAN_SIZE - 1 - i; j++) {
      if (tmp[j] > tmp[j + 1]) {
        float t = tmp[j]; tmp[j] = tmp[j + 1]; tmp[j + 1] = t;
      }
    }
  }
  return tmp[MEDIAN_SIZE / 2];
}

适用： 超声波偶尔跳变、红外测距、按键消抖

三、一阶低通滤波（RC 滤波）

模拟 RC 低通电路的数字化版本，最常用的实时滤波器。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
/* 一阶低通滤波 */
typedef struct {
  float alpha;  // 滤波系数 0~1，越小越平滑
  float last;
} LowPass;

float LowPass_Update(LowPass *f, float x) {
  f->last = f->alpha * x + (1 - f->alpha) * f->last;
  return f->last;
}

/* 使用示例 */
LowPass lp = { .alpha = 0.15f, .last = 0 };

while (1) {
  float raw = Read_AD9700();           // 原始 ADC
  float filtered = LowPass_Update(&lp, raw);  // 滤波后
  printf("raw=%.2f  filtered=%.2f\r\n", raw, filtered);
  HAL_Delay(10);
}

截止频率计算

1
2
3
4
5
6
7
8
alpha = T / (T + RC)

其中 T = 采样周期，RC = 时间常数

截止频率: fc = 1 / (2π × RC) = alpha / (2π × T × (1-alpha))

例: T=10ms, alpha=0.15
    → fc ≈ 0.15 / (2π × 0.01 × 0.85) ≈ 2.8Hz
alpha 平滑程度 截止频率（@10ms采样） 适用场景
0.5 较弱 ~16Hz 轻度去抖
0.15 中等 ~2.8Hz 姿态角度（推荐）
0.05 很强 ~0.8Hz 温度等慢变信号

四、高通滤波

保留变化快的分量，滤除直流偏置。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
/* 一阶高通滤波 */
typedef struct {
  float alpha;
  float last_input;
  float last_output;
} HighPass;

float HighPass_Update(HighPass *f, float x) {
  f->last_output = f->alpha * (f->last_output + x - f->last_input);
  f->last_input = x;
  return f->last_output;
}

/* 使用：滤除加速度计的重力分量 */
HighPass hp = { .alpha = 0.9f };
float accel_without_gravity = HighPass_Update(&hp, raw_accel);

五、互补滤波

原理

陀螺仪高频准但低频漂，加速度计低频准但高频抖。

互补滤波取各自长处：

1
2
3
4
5
6
7
8
9
10
11
12
/* 一阶互补滤波（单轴） */
typedef struct {
  float alpha;  // 陀螺仪权重，通常 0.96~0.99
  float angle;
} Complementary;

float Complementary_Update(Complementary *f,
    float gyro_rate, float accel_angle, float dt) {
  f->angle = f->alpha * (f->angle + gyro_rate * dt)
           + (1 - f->alpha) * accel_angle;
  return f->angle;
}

α 取值指南

alpha 特点 适用
0.99 非常信任陀螺仪，响应慢 高帧率运动
0.98 平衡点 大多数情况
0.95 更依赖加速度计，响应快 低速静态姿态

C 语言完整实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
/* 互补滤波 + 角度归一化 */
#define PI  3.14159265f
#define RAD2DEG(x)  ((x) * 180.0f / PI)

typedef struct {
  float alpha;       // 权重系数
  float dt;          // 采样周期 (s)
  float pitch, roll; // 输出角度
} ComplementaryFilter;

void CompFilter_Init(ComplementaryFilter *f, float alpha, float dt) {
  f->alpha = alpha;
  f->dt = dt;
  f->pitch = 0;
  f->roll = 0;
}

void CompFilter_Update(ComplementaryFilter *f,
    float ax, float ay, float az,
    float gx, float gy, float gz) {

  /* 加速度计算角度 */
  float accel_pitch = atan2f(-ax, sqrtf(ay * ay + az * az));
  float accel_roll  = atan2f(ay, az);

  /* 互补滤波 */
  f->pitch = f->alpha * (f->pitch + gx * f->dt)
           + (1 - f->alpha) * accel_pitch;
  f->roll  = f->alpha * (f->roll  + gy * f->dt)
           + (1 - f->alpha) * accel_roll;
}

/* 使用 */
ComplementaryFilter cf;
CompFilter_Init(&cf, 0.98f, 0.004f);  // 250Hz 采样

while (1) {
  MPU6050_Read(&mpu);
  CompFilter_Update(&cf,
    mpu.ax, mpu.ay, mpu.az,
    mpu.gx, mpu.gy, mpu.gz);
  printf("Pitch=%.1f  Roll=%.1f\r\n",
    RAD2DEG(cf.pitch), RAD2DEG(cf.roll));
  HAL_Delay(4);
}

六、卡尔曼滤波

核心思想

卡尔曼滤波不是”滤掉噪声”，而是动态估计最可能的状态。

看起来复杂，拆成代码其实很清晰。

一维卡尔曼滤波（单变量）

适合展示原理、温度平滑、电压跟踪。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
/* 一维卡尔曼滤波 */
typedef struct {
  float Q;      // 过程噪声协方差（系统稳定性）
  float R;      // 测量噪声协方差（传感器信任度）
  float x;      // 估计值
  float p;      // 估计误差协方差
  float k;      // 卡尔曼增益
} Kalman1D;

void Kalman1D_Init(Kalman1D *k, float Q, float R, float init_x) {
  k->Q = Q;
  k->R = R;
  k->x = init_x;
  k->p = 1.0f;
}

float Kalman1D_Update(Kalman1D *k, float z) {
  /* 预测 */
  k->p = k->p + k->Q;

  /* 更新 */
  k->k = k->p / (k->p + k->R);
  k->x = k->x + k->k * (z - k->x);
  k->p = (1 - k->k) * k->p;

  return k->x;
}

/* 使用 */
Kalman1D kf;
Kalman1D_Init(&kf, 0.001f, 0.1f, 0);

float filtered_temp = Kalman1D_Update(&kf, raw_temp);

Q/R 调参指南

Q 增大 R 增大
更信任测量值 更信任预测值
响应快、但有噪声 平滑、但有延迟
适合快速变化信号 适合慢变信号

经验公式： R / Q ≈ 传感器信噪比的倒数

二维卡尔曼滤波（角度跟踪）

MPU6050 常用的 2 状态卡尔曼（角度 + 角速度偏差）。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
/* 二维卡尔曼（角度 + 陀螺偏置） */
typedef struct {
  float Q_angle;    // 角度过程噪声
  float Q_gyro;     // 角速度偏置过程噪声
  float R_angle;    // 角度测量噪声
  float angle;      // 输出角度
  float bias;       // 陀螺偏置
  float P[2][2];    // 协方差矩阵
} Kalman2D;

void Kalman2D_Init(Kalman2D *k) {
  k->Q_angle = 0.001f;
  k->Q_gyro  = 0.003f;
  k->R_angle = 0.03f;
  k->angle = 0;
  k->bias  = 0;
  k->P[0][0] = 0; k->P[0][1] = 0;
  k->P[1][0] = 0; k->P[1][1] = 0;
}

float Kalman2D_Update(Kalman2D *k, float new_angle, float gyro_rate, float dt) {
  /* 预测 */
  k->angle += dt * (gyro_rate - k->bias);
  k->P[0][0] += dt * (dt * k->P[1][1] - k->P[0][1] - k->P[1][0] + k->Q_angle);
  k->P[0][1] -= dt * k->P[1][1];
  k->P[1][0] -= dt * k->P[1][1];
  k->P[1][1] += k->Q_gyro * dt;

  /* 更新 */
  float S = k->P[0][0] + k->R_angle;
  float K0 = k->P[0][0] / S;
  float K1 = k->P[1][0] / S;

  float y = new_angle - k->angle;
  k->angle += K0 * y;
  k->bias   += K1 * y;

  float P00_temp = k->P[0][0];
  float P01_temp = k->P[0][1];

  k->P[0][0] -= K0 * P00_temp;
  k->P[0][1] -= K0 * P01_temp;
  k->P[1][0] -= K1 * P00_temp;
  k->P[1][1] -= K1 * P01_temp;

  return k->angle;
}

/* 使用：替代互补滤波 */
Kalman2D kalman_pitch, kalman_roll;
Kalman2D_Init(&kalman_pitch);
Kalman2D_Init(&kalman_roll);

float pitch = Kalman2D_Update(&kalman_pitch, accel_pitch, gyro_x, 0.004f);
float roll  = Kalman2D_Update(&kalman_roll,  accel_roll,  gyro_y, 0.004f);

七、滤波器对比

特性 滑动平均 中值滤波 一阶低通 互补滤波 卡尔曼滤波
计算量 ★☆☆☆☆ ★★★☆☆ ★★☆☆☆ ★★☆☆☆ ★★★★☆
内存 中 中 极小 小 中
延迟 有 有 小 小 极小
抗脉冲 差 极好 一般 一般 好
自适应 否 否 否 否 是
调参难度 无 无 低 低 高
代码量 15行 20行 8行 20行 50行

八、选型与流程

九、实际案例：MPU6050 数据处理链

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/* 完整数据链 */
LowPass lp_accel = { .alpha = 0.2f };
MovingAvg avg = { 0 };
Complementary comp = { .alpha = 0.98f, .angle = 0 };

void MPU_Process(float ax, float ay, float az,
                 float gx, float gy, float gz, float dt) {
  /* 1. 加速度计低通 */
  ax = LowPass_Update(&lp_accel, ax);
  ay = LowPass_Update(&lp_accel, ay);
  az = LowPass_Update(&lp_accel, az);

  /* 2. 角度计算 */
  float accel_pitch = atan2f(-ax, sqrtf(ay*ay + az*az));

  /* 3. 滑动平均去抖 */
  accel_pitch = MovingAvg_Update(&avg, accel_pitch);

  /* 4. 互补滤波融合 */
  float pitch = Complementary_Update(&comp, gx, accel_pitch, dt);
}

参考