3、卡尔曼滤波入门：状态空间模型、预测与更新方程、一维温度滤波实战

各位同学，欢迎来到第三讲。

前两章我们聊了传感器噪声、数据融合的基本思路。今天要讲的卡尔曼滤波，可以说是嵌入式数据融合领域的「瑞士军刀」。我做了十几年嵌入式，处理过无数传感器数据，卡尔曼滤波是我工具箱里最趁手的家伙之一。

说白了，卡尔曼滤波就是一套数学公式，它能从一堆带噪声的测量值里，把真实状态给「猜」出来。而且它算得快，适合在单片机上跑。嗯，咱们今天就从最基础的一维温度滤波开始，把它的核心思想掰开揉碎了讲清楚。

3.1 状态空间模型：把问题装进数学框子里

要理解卡尔曼滤波，先得理解它看待世界的方式。它把任何动态系统都抽象成两个模型：

• 状态方程：描述系统内部状态怎么随时间变化。
• 观测方程：描述我们怎么通过传感器看到这个状态。

拿一维温度滤波举例。假设你有一个温度传感器，每秒采集一次温度。真实温度是 \(x_k\)，传感器读数是 \(z_k\)。

状态方程可以写成：

x_k = x_{k-1} + w_k

这里 \(w_k\) 是过程噪声——温度本身会有微小波动，比如空气流动、热源干扰。我习惯把它设成高斯白噪声，均值0，方差 \(Q\)。

观测方程：

z_k = x_k + v_k

\(v_k\) 是测量噪声，来自传感器自身精度限制。方差记作 \(R\)。

你看，就这么简单。真实温度是我们要估计的「状态」，传感器读数是我们能拿到的「观测」。卡尔曼滤波要做的，就是利用这两个方程，把真实温度从噪声里「滤」出来。

3.2 预测与更新方程：卡尔曼滤波的两步舞

卡尔曼滤波就两个步骤，循环往复：预测和更新。

3.2.1 预测步骤（Time Update）

这一步，我们用状态方程，根据上一时刻的最优估计，预测当前时刻的状态。

// 预测状态
x_pred = x_est_prev

// 预测误差协方差
P_pred = P_est_prev + Q

这里 \(x_{pred}\) 是预测值，\(P_{pred}\) 是预测的不确定性（方差）。Q 是过程噪声方差，它会让预测的不确定性随时间增大——你想想看，如果模型不准，预测时间越长，结果越不可靠。

我记得第一次在STM32上实现时，Q设得太小，滤波结果几乎不响应温度变化，反应迟钝。后来把Q调大了一点，效果就好多了。

3.2.2 更新步骤（Measurement Update）

拿到传感器新读数后，我们用观测方程来修正预测值。

// 卡尔曼增益
K = P_pred / (P_pred + R)

// 更新状态估计
x_est = x_pred + K * (z - x_pred)

// 更新误差协方差
P_est = (1 - K) * P_pred

卡尔曼增益 \(K\) 是关键。它决定了我们更相信预测值还是测量值：

• 如果测量噪声小（R小），K接近1，我们更相信传感器读数。
• 如果预测很准（P_pred小），K接近0，我们更相信模型预测。

说白了，卡尔曼增益就是一个动态权重，它根据当前的不确定性自动调节。

3.3 一维温度滤波实战：从公式到代码

理论讲完了，咱们直接上代码。下面是一个完整的一维卡尔曼滤波实现，用C语言写的，可以直接移植到任何嵌入式平台上。

#include <stdio.h>

typedef struct {
    float Q;       // 过程噪声方差
    float R;       // 测量噪声方差
    float x;       // 状态估计值
    float P;       // 估计误差协方差
    float K;       // 卡尔曼增益
} KalmanFilter;

// 初始化卡尔曼滤波器
void Kalman_Init(KalmanFilter *kf, float init_temp) {
    kf->Q = 0.01f;      // 过程噪声，根据实际情况调整
    kf->R = 0.25f;      // 测量噪声，来自传感器精度
    kf->x = init_temp;  // 初始温度估计
    kf->P = 1.0f;       // 初始估计误差，设大一点没关系
}

// 执行一步卡尔曼滤波
float Kalman_Update(KalmanFilter *kf, float measurement) {
    // 预测
    float x_pred = kf->x;           // 状态预测
    float P_pred = kf->P + kf->Q;   // 协方差预测

    // 更新
    kf->K = P_pred / (P_pred + kf->R);          // 卡尔曼增益
    kf->x = x_pred + kf->K * (measurement - x_pred); // 状态更新
    kf->P = (1.0f - kf->K) * P_pred;            // 协方差更新

    return kf->x;
}

int main() {
    KalmanFilter kf;
    Kalman_Init(&kf, 25.0f);  // 假设初始温度25°C

    // 模拟一组带噪声的温度测量值
    float measurements[] = {25.1f, 25.3f, 24.8f, 25.2f, 25.0f, 24.9f, 25.4f, 25.1f};
    int n = sizeof(measurements) / sizeof(measurements[0]);

    printf("原始测量值\t滤波后值\n");
    for (int i = 0; i < n; i++) {
        float filtered = Kalman_Update(&kf, measurements[i]);
        printf("%.2f\t\t%.2f\n", measurements[i], filtered);
    }

    return 0;
}

运行这段代码，你会看到滤波后的温度值比原始测量值平滑很多。这就是卡尔曼滤波的威力——它把噪声给「吃」掉了。

3.4 参数调优：让滤波器为你工作

卡尔曼滤波好不好用，全看参数调得怎么样。下面这个表格是我多年总结的经验值：

参数	物理含义	调大效果	调小效果	典型值范围
Q	过程噪声方差	更跟随测量值，响应快	更平滑，响应慢	0.001 ~ 0.1
R	测量噪声方差	更相信预测值，平滑	更相信测量值，波动大	0.01 ~ 1.0
P_init	初始估计误差	收敛慢	收敛快（但可能不准）	0.1 ~ 10.0

调参没有银弹。我个人的习惯是：先用传感器数据手册上的精度值作为R的初始值，然后现场调试Q。调的时候，让传感器快速变化（比如用手捂一下），看滤波结果跟不跟得上。跟得太慢就增大Q，抖得太厉害就减小Q。

3.5 本章小结

这一章我们干了三件事：

• 理解了状态空间模型——把温度滤波问题装进数学框子。
• 掌握了预测与更新方程——卡尔曼滤波的两步舞。
• 实战了一维温度滤波——从公式到C代码，再到参数调优。

卡尔曼滤波入门其实不难，难的是理解它背后的思想——用不确定性来指导融合。下一章我们会把这个一维滤波器扩展到多维，处理更复杂的传感器数据。到时候你会发现，核心思想完全一样，只是矩阵运算代替了标量运算。

嗯，今天就到这里。回去把代码跑一跑，调一调参数，感受一下卡尔曼滤波的魔力。有任何问题，咱们下节课见。