第4章 卡尔曼滤波入门：状态空间模型、预测与更新步骤、一维卡尔曼滤波实现
说实话，很多做传感器融合的工程师一听到「卡尔曼滤波」就头大。我当年也一样，看了好几篇，公式推导看得云里雾里。后来我发现，其实卡尔曼滤波没那么玄乎——它就是一个聪明的「猜数字」游戏。
你想想看，我们手上有两个信息源：一个是系统的物理规律（比如物体应该匀速运动），另一个是传感器的测量值（比如GPS报的位置）。这两个信息都有误差，卡尔曼滤波要做的，就是把它们「揉」在一起，得到一个比任何一个都准的结果。
4.1 状态空间模型：把问题装进数学框里
做卡尔曼滤波的第一步，就是建立状态空间模型。说白了，就是回答两个问题：
- 系统是怎么演变的？（状态方程）
- 我们能观测到什么？（观测方程）
拿一维温度测量来举例。假设我们想估计一个房间的温度，温度变化很慢，基本可以看作常数。那么状态方程就是：
x(k) = x(k-1) + w(k-1)这里的 x(k) 是 k 时刻的真实温度，w(k-1) 是过程噪声——比如有人开了窗、空调启动了。我习惯把 w 看作「我们模型没考虑到的扰动」。
观测方程呢？我们用温度传感器去测，得到：
z(k) = x(k) + v(k)z(k) 是测量值，v(k) 是测量噪声——传感器本身的误差、ADC量化误差等等。
关键点：状态空间模型的核心就是两个方程。一个描述「真实世界怎么变」，一个描述「我们能看到什么」。剩下的，就是卡尔曼滤波的活了。
4.2 预测与更新：卡尔曼滤波的两步舞
卡尔曼滤波就两个步骤，循环往复：预测和更新。我当年在做一个无人机高度估计项目时，就是靠这两步把气压计和加速度计的数据融合起来的。
4.2.1 预测步骤（Time Update）
预测，就是根据上一时刻的最优估计，猜一下当前时刻的状态。公式很简单：
x_pred = x_est_prev // 状态预测
P_pred = P_est_prev + Q // 误差协方差预测这里：
- x_pred 是预测的状态（先验估计）
- P_pred 是预测的误差协方差——代表我们对预测的「信心」
- Q 是过程噪声协方差，代表我们对模型的信任程度
嗯，这里要注意：Q 越大，说明我们越不相信模型，卡尔曼滤波就会更依赖测量值。我在调参时，经常把 Q 设成一个小值（比如 0.01），然后慢慢往上加，看效果。
4.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，我们几乎完全相信测量值
- 如果 R 很大（测量噪声大），K 接近 0，我们更相信预测
我的经验：刚开始调卡尔曼滤波时，先固定 Q 和 R 的值，观察 K 的变化趋势。如果 K 一直很大（接近1），说明测量噪声设得太小了；如果 K 一直很小，说明过程噪声设得太小了。我曾经在一个惯性导航项目里，就是因为 R 设得太小，导致滤波结果跟着测量值剧烈抖动，后来把 R 调大了一倍，曲线就平滑了。
4.3 一维卡尔曼滤波实现：手把手写代码
理论说完了，咱们直接上代码。这是一个完整的一维卡尔曼滤波实现，用来估计一个缓慢变化的温度值。
#include <stdio.h>
typedef struct {
float x; // 状态估计值
float P; // 误差协方差
float Q; // 过程噪声协方差
float R; // 测量噪声协方差
} KalmanFilter;
// 初始化卡尔曼滤波器
void kalman_init(KalmanFilter *kf, float init_x, float init_P, float Q, float R) {
kf->x = init_x;
kf->P = init_P;
kf->Q = Q;
kf->R = R;
}
// 预测步骤
void kalman_predict(KalmanFilter *kf) {
// 状态预测：x = x（因为是常数模型）
// 协方差预测：P = P + Q
kf->P = kf->P + kf->Q;
}
// 更新步骤
float kalman_update(KalmanFilter *kf, float measurement) {
// 计算卡尔曼增益
float K = kf->P / (kf->P + kf->R);
// 更新状态估计
kf->x = kf->x + K * (measurement - kf->x);
// 更新误差协方差
kf->P = (1 - K) * kf->P;
return kf->x;
}
int main() {
KalmanFilter kf;
// 初始化：初始温度25度，初始误差1.0，Q=0.01，R=0.1
kalman_init(&kf, 25.0f, 1.0f, 0.01f, 0.1f);
// 模拟测量数据（真实温度26度，加了一些噪声）
float measurements[] = {25.5f, 26.2f, 25.8f, 26.5f, 26.1f};
int n = sizeof(measurements) / sizeof(measurements[0]);
printf("时刻\t测量值\t滤波值\n");
for (int i = 0; i < n; i++) {
kalman_predict(&kf);
float filtered = kalman_update(&kf, measurements[i]);
printf("%d\t%.2f\t%.2f\n", i+1, measurements[i], filtered);
}
return 0;
}运行这段代码，你会看到滤波值比测量值更平滑，而且逐渐收敛到真实值附近。这就是卡尔曼滤波的魔力。
避坑指南：我曾经犯过一个低级错误——在每次更新后忘了更新 P 值，结果滤波结果发散得一塌糊涂。还有一次，我把 Q 和 R 设反了，滤波结果比原始测量还差。记住：Q 控制模型信任度，R 控制测量信任度，千万别搞混。
4.4 参数调优：Q 和 R 的玄学
说实话，卡尔曼滤波最难的不是公式，而是调参。Q 和 R 怎么设？我总结了几个经验：
| 参数 | 物理意义 | 调大效果 | 调小效果 |
|---|---|---|---|
| Q（过程噪声） | 模型的不确定性 | 更相信测量，响应快但噪声大 | 更相信模型，响应慢但平滑 |
| R（测量噪声） | 传感器的不确定性 | 更相信模型，平滑但滞后 | 更相信测量，响应快但抖动 |
| P（初始协方差） | 初始估计的不确定性 | 初始收敛慢 | 初始收敛快，但可能过冲 |
我的调参习惯是：先根据传感器数据手册估算 R（比如温度传感器精度±0.5°C，R 可以设成 0.25），然后 Q 从 0.001 开始试。如果滤波结果太「死板」（跟不上变化），就增大 Q；如果太「活泼」（抖动厉害），就减小 Q。
4.5 可视化：卡尔曼滤波的迭代过程
为了让你更直观地理解卡尔曼滤波的迭代过程，我画了一张流程图：
这张图把卡尔曼滤波的迭代过程讲得很清楚。每次循环，先预测，再更新，然后输出最优估计。下一时刻，把当前的最优估计作为初始值，继续循环。
核心总结：卡尔曼滤波的本质就是一个「预测-校正」的闭环。预测给出「我认为应该是多少」，测量给出「我看到的是多少」，卡尔曼增益决定「我该信谁多一点」。一维情况搞懂了，多维卡尔曼滤波就是在这个基础上加矩阵运算而已。
好了，这一章的内容就到这里。代码你可以直接拿去用，调参的时候多试试不同的 Q 和 R 组合，感受一下它们对滤波效果的影响。相信我，亲手调一遍比看十遍理论都管用。