卡尔曼滤波：嵌入式工程师落地指南

一句话概括：卡尔曼滤波 = 用模型先预测一次，再用传感器测量值修正一次，并且自动判断“更相信模型还是更相信传感器”。

1. 先用一个生活例子理解

假设你用一个温度传感器测环境温度。真实温度可能是 25.0 ℃，但传感器每次读数会抖动：

1
2
3
4
5
24.7 ℃
25.3 ℃
24.9 ℃
25.4 ℃
24.8 ℃

你不能每次都直接相信传感器，否则显示值会乱跳。
一个最简单的办法是做平均滤波：

1
filtered = 0.9 * filtered + 0.1 * measured;   // 一阶低通

但卡尔曼滤波更聪明：它不只是固定用 0.1 或 0.9，而是动态计算一个权重——这一次我更应该相信预测值，还是更相信传感器？这个权重就叫 卡尔曼增益 K。

2. 卡尔曼滤波的核心思想

每一次循环都做两件事：

  1. 预测：根据上一次状态，预测当前状态
  2. 修正：用传感器测量值，对预测值进行修正

例如温度变化很慢，你可以预测“当前温度 ≈ 上一次温度”，但传感器告诉你一个新值（比如 26.0 ℃）。
问题来了：预测值 25.0，测量值 26.0，最终取多少？

  • 如果传感器很准 → 靠近 26.0
  • 如果传感器噪声很大 → 靠近 25.0

卡尔曼滤波就是做这个权衡。

3. 先看最简单的一维卡尔曼滤波

一维场景很常见：温度、电压、ADC、距离、压力、电池电量等。

3.1 状态变量 x

当前估计值（如温度 25.0）。

3.2 测量值 z

传感器读到的值（如 26.0）。

3.3 估计误差 P

对当前估计值的不确定度。P 越大，越不确定。

3.4 过程噪声 Q

系统本身变化的不确定性（如温度真的在波动）。Q 越大，认为系统本身变化越剧烈。

3.5 测量噪声 R

传感器噪声大小。R 越大，表示传感器越不可靠。

3.6 卡尔曼增益 K

本次修正时测量值占的权重。K 越大，越相信测量值；K 越小，越相信预测值。

4. 一维卡尔曼滤波公式

第一步：预测

1
2
x_pre = x
P_pre = P + Q

预测当前状态 = 上一次状态；预测误差 = 上一次误差 + 系统变化带来的新不确定性。

第二步：计算卡尔曼增益

1
K = P_pre / (P_pre + R)
  • 如果 P_pre 很大（预测不可靠）→ K 变大 → 更相信测量
  • 如果 R 很大（传感器不可靠）→ K 变小 → 更相信预测

第三步：修正估计值

1
x = x_pre + K * (z - x_pre)

其中 (z - x_pre) 叫残差（创新量），表示测量值与预测值的差。

第四步：更新误差

1
P = (1 - K) * P_pre

修正后不确定性通常会下降。

5. 数值例子（温度滤波）

初始条件：

  • 上一次估计温度 x = 25.0 ℃
  • 估计误差 P = 1.0
  • 过程噪声 Q = 0.01
  • 测量噪声 R = 0.25
  • 本次传感器读数 z = 26.0 ℃

预测：

1
2
x_pre = 25.0
P_pre = 1.0 + 0.01 = 1.01

卡尔曼增益：

1
K = 1.01 / (1.01 + 0.25) = 1.01 / 1.26 ≈ 0.802

K 很大，说明这次比较相信传感器。

修正估计值：

1
x = 25.0 + 0.802 * (26.0 - 25.0) = 25.802 ℃

最终输出不是直接跳到 26.0，而是 25.802。

更新误差：

1
P = (1 - 0.802) * 1.01 ≈ 0.200

P 变小，滤波器对当前估计更有信心了。

6. 卡尔曼滤波 vs 普通低通滤波

普通一阶低通卡尔曼滤波
y = a*y + (1-a)*xestimate = estimate + K*(measurement - estimate)
权重 a 固定权重 K 动态计算
简单，但无法自适应根据不确定性动态调整

7. 一维卡尔曼滤波 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
typedef struct {
    float x;   // 当前估计值
    float p;   // 当前估计误差
    float q;   // 过程噪声
    float r;   // 测量噪声
    float k;   // 卡尔曼增益
} kalman_1d_t;

void kalman_1d_init(kalman_1d_t *kf, float init_x, float init_p, float q, float r) {
    if (kf == NULL) return;
    kf->x = init_x;
    kf->p = init_p;
    kf->q = q;
    kf->r = r;
    kf->k = 0.0f;
}

float kalman_1d_update(kalman_1d_t *kf, float measurement) {
    if (kf == NULL) return 0.0f;

    // 1. 预测
    float x_pre = kf->x;
    float p_pre = kf->p + kf->q;

    // 2. 计算卡尔曼增益
    kf->k = p_pre / (p_pre + kf->r);

    // 3. 修正估计值
    kf->x = x_pre + kf->k * (measurement - x_pre);

    // 4. 更新估计误差
    kf->p = (1.0f - kf->k) * p_pre;

    return kf->x;
}

使用示例：

1
2
3
4
5
6
7
8
kalman_1d_t temp_filter;
kalman_1d_init(&temp_filter, 25.0f, 1.0f, 0.01f, 0.25f);

while (1) {
    float raw_temp = read_temperature_sensor();
    float filtered_temp = kalman_1d_update(&temp_filter, raw_temp);
    printf("raw = %.2f, filtered = %.2f\r\n", raw_temp, filtered_temp);
}

8. Q 和 R 怎么调？（工程关键）

参数含义调参方向
R（测量噪声）传感器噪声大小传感器抖动大 → 增大 R（更平滑，但响应变慢）
传感器很准 → 减小 R（响应更快）
Q（过程噪声）系统本身变化快慢目标变化快 → 增大 Q（更灵敏）
目标变化慢 → 减小 Q（更稳定）

实用口诀：

  • 滤波后还很抖 → 增大 R 或 减小 Q
  • 响应太慢 → 减小 R 或 增大 Q
  • 传感器很准 → R 小一些
  • 目标变化快 → Q 大一些

9. 嵌入式中常见应用

应用场景适用滤波器
ADC电压、NTC温度、电流采样、压力、光照一维卡尔曼
超声波/ToF/红外测距一维卡尔曼（仅距离）
电机编码器测速（速度抖动）一维卡尔曼（仅速度）
同时估计位置+速度二维卡尔曼
IMU姿态（加速度计+陀螺仪）互补滤波 / Mahony / EKF（进阶）

10. 二维例子：同时估计位置和速度

状态向量：

1
X = [位置, 速度]^T

运动模型（匀速，采样周期 dt）：

1
2
当前位置 = 上一位置 + 上一速度 * dt
当前速度 = 上一速度

矩阵形式：

1
2
3
X_pre = A * X
A = [1  dt
     0  1 ]

例：dt = 0.1s，上一时刻位置=10m，速度=2m/s
预测当前：

1
2
新位置 = 10 + 2*0.1 = 10.2 m
新速度 = 2 m/s

若传感器测到位置 z = 10.4 m，滤波器仍通过卡尔曼增益决定相信预测还是测量。

11. 完整线性卡尔曼滤波公式（矩阵形式）

预测：

1
2
X_pre = A * X + B * U
P_pre = A * P * A^T + Q

更新：

1
2
3
K = P_pre * H^T * inv(H * P_pre * H^T + R)
X = X_pre + K * (Z - H * X_pre)
P = (I - K * H) * P_pre

理解：

  • X_pre = 预测值
  • Z - H*X_pre = 测量值和预测值的差
  • K = 修正比例
  • X = 预测值 + 修正比例 × 差值

12. 卡尔曼滤波的本质

  • 它不只是“让数据变平滑”，而是状态估计。
  • 融合两类信息：系统模型（预测）和传感器测量（观测）。
  • 根据二者的不确定性，动态融合出最优估计。

适用场景：

  • 有噪声的传感器
  • 有一定运动模型的系统
  • 需要实时状态估计（位置、速度、姿态、SOC等）

13. 常见误区

误区正解
卡尔曼滤波一定比平均滤波好对于简单ADC抖动，滑动平均可能更简单高效
Q 和 R 随便填它们决定滤波器性格，需根据实际调参
能消除所有噪声只能在模型合理时给出更优估计，不能消除全部噪声
P 不重要P 代表滤波器自信度，初始化过小会导致前期响应极慢

14. 嵌入式滤波方案选择建议

需求推荐方案
普通ADC抖动滑动平均 / 一阶低通 / 一维卡尔曼
信号变化慢，有噪声一维卡尔曼
需同时估计位置+速度二维卡尔曼
多传感器融合多维卡尔曼 / EKF
系统非线性明显EKF / UKF
资源极其有限一阶低通优先

15. 实战建议（第一次上手）

  1. 先用一维卡尔曼处理一个简单传感器（如ADC、温度、距离）。
  2. 固定 Q 和 R，不要一开始就做自适应。
  3. 打印原始值与滤波值对比，观察效果。
  4. 根据效果调参：
    • 太抖 → 增大 R
    • 太慢 → 增大 Q 或 减小 R
  5. 保证采样周期 dt 稳定，尤其当模型涉及速度、加速度时。

16. 最后一句总结

卡尔曼滤波 = 根据上一次状态预测当前值 + 看看传感器测到多少 + 根据“预测不准”和“测量不准”自动决定修正多少。

嵌入式最常用的一维核心代码：

1
2
3
4
5
6
7
x_pre = x;
p_pre = p + q;

k = p_pre / (p_pre + r);

x = x_pre + k * (measurement - x_pre);
p = (1 - k) * p_pre;

这一版已足够解决大量 ADC、温度、电压、电流、距离、速度类信号的滤波问题。

以上内容来源于ChatGPT，仅供参考