3、卡尔曼滤波入门：状态空间模型、预测与更新步骤、协方差矩阵的意义
卡尔曼滤波，说白了就是一套「猜得准」的数学方法。
我刚开始接触它的时候，觉得这东西玄乎得很。后来在目标跟踪项目里被噪声折磨得够呛，才真正体会到它的价值。你想想看，传感器测出来的数据总是带噪声的——雷达可能跳一下，摄像头可能丢一帧。卡尔曼滤波干的事就是：把「不太准的测量」和「不太准的预测」揉在一起，揉出一个更准的结果。
3.1 状态空间模型：你在跟踪什么？
先问一个问题：你要描述一个运动的目标，最少需要几个数？
位置？对。速度？也对。如果目标在二维平面上运动，那就是四个数：x位置、y位置、x方向速度、y方向速度。这组数，就是状态向量。
状态空间模型的核心思想：把系统抽象成一组随时间演变的变量，用数学方程描述它们的变化规律。
我个人习惯把状态空间模型拆成两半来看：
- 状态方程：描述状态怎么随时间变化。比如「下一时刻的位置 = 当前位置 + 速度 × 时间间隔」。
- 观测方程：描述我们能测到什么。比如「雷达测到的距离 = 真实距离 + 测量噪声」。
用数学写出来就是：
x(k) = A · x(k-1) + B · u(k) + w(k) // 状态方程
z(k) = H · x(k) + v(k) // 观测方程这里：
x(k)是 k 时刻的状态向量A是状态转移矩阵——描述上一时刻的状态怎么变到这一时刻B是控制输入矩阵——如果你有油门、刹车这类控制量u(k)是控制输入w(k)是过程噪声——模型不完美带来的误差z(k)是观测值H是观测矩阵——把状态映射到观测空间v(k)是观测噪声——传感器本身的误差
嗯，这里要注意：w(k) 和 v(k) 都是随机变量，我们假设它们服从高斯分布。为什么？因为高斯分布数学性质好，算起来方便。现实世界不一定完美高斯，但够用了。
3.2 预测与更新：两步走，循环往复
卡尔曼滤波就两个步骤，像个永不停歇的循环：
- 预测（Predict）：根据上一时刻的状态，猜一下当前时刻的状态
- 更新（Update）：拿到当前时刻的测量值，修正刚才的猜测
我在项目中遇到过这样的情况：雷达每 50ms 发一帧数据，但中间有丢帧。卡尔曼滤波的预测步骤就能帮我「脑补」出丢帧期间目标的位置。等下一帧数据来了，更新步骤再把它拉回真实轨迹。
预测步骤
x_pred = A · x_prev + B · u // 预测状态
P_pred = A · P_prev · A^T + Q // 预测协方差这里多了一个 P 和 Q。P 是协方差矩阵，代表我们对当前状态估计的不确定度。Q 是过程噪声协方差矩阵，代表模型本身的不确定性。
更新步骤
K = P_pred · H^T · (H · P_pred · H^T + R)^(-1) // 卡尔曼增益
x_updated = x_pred + K · (z - H · x_pred) // 更新状态
P_updated = (I - K · H) · P_pred // 更新协方差这个 K 就是卡尔曼增益，它是整个算法的灵魂。它决定了：你更相信预测，还是更相信测量？
我的经验：卡尔曼增益 K 的值在 0 到 1 之间。K 接近 0 时，说明测量噪声大，我更相信预测。K 接近 1 时，说明预测不准，我更相信测量。调参的时候，调整 Q 和 R 的比值，本质上就是在调这个 K 的行为。
3.3 协方差矩阵的意义：不确定度的「地图」
协方差矩阵 P 可能是初学者最头疼的东西。我刚开始也绕了很久，后来想明白了一件事：它其实就是一张「不确定度的地图」。
假设我们只跟踪一维位置 x 和速度 v，那么协方差矩阵长这样：
P = | P_xx P_xv |
| P_vx P_vv |P_xx：位置的不确定度（方差）P_vv：速度的不确定度（方差）P_xv和P_vx：位置和速度的相关性——如果位置偏大，速度是不是也偏大？
为什么这个矩阵重要？
举个例子。假设你跟踪一辆匀速行驶的车。如果位置不确定度很大，但速度不确定度很小，那说明你知道车大概跑多快，但不知道它具体在哪。这时候，卡尔曼滤波会倾向于用速度来推算位置——因为速度更可信。
反过来，如果位置很准但速度不准，那滤波就会更依赖位置测量来修正速度估计。
我曾经踩过的坑：一开始我把协方差矩阵的初始值设得特别小，觉得「我的初始猜测很准」。结果滤波器收敛极慢，因为初始的 P 太小，导致卡尔曼增益 K 一直很小，测量值几乎不起作用。后来我学乖了：初始 P 设大一点，让滤波器先「大胆尝试」，再慢慢收敛。
3.4 一张图看懂卡尔曼滤波
下面这张 SVG 图展示了卡尔曼滤波的完整流程。我建议你盯着它看一分钟，把预测和更新的循环刻在脑子里。
3.5 调参心得：Q 和 R 的博弈
卡尔曼滤波里最让人头疼的就是调参。说白了就是调两个东西：
- 过程噪声协方差 Q：模型有多不准？目标突然加速了？转弯了？Q 设大一点，滤波器会更「灵活」。
- 测量噪声协方差 R：传感器有多不准？R 设大一点，滤波器会更「平滑」。
我个人的经验是：先固定 R（根据传感器手册上的噪声指标来设），然后调 Q。Q 太小，滤波结果会滞后；Q 太大，结果会抖动。找到一个平衡点，让滤波后的轨迹既平滑又能跟上真实运动。
一句话总结：卡尔曼滤波就是「预测 + 测量 → 加权平均」，权重由协方差矩阵自动计算。你只需要告诉它：你的模型有多可信（Q），你的传感器有多可信（R）。剩下的，它自己搞定。
嗯，这一章的内容就到这里。协方差矩阵这个东西，光看公式确实抽象。我建议你动手写一个一维的卡尔曼滤波代码，跟踪一个匀速运动的目标。跑起来之后，打印出每一步的 P 矩阵，看着它慢慢收敛——那种感觉，比看十遍公式都管用。