3、卡尔曼滤波与决策：卡尔曼滤波基本原理、状态预测与更新、卡尔曼增益的决策意义、协方差矩阵的作用
各位同学，今天我们来聊聊卡尔曼滤波。说实话，这玩意儿在跟踪领域里，就像炒菜离不开盐一样——太基础了，也太重要了。我当年刚入行时，总觉得卡尔曼滤波就是个数学公式堆砌的东西，直到在项目中吃了亏，才真正理解它的精髓。
卡尔曼滤波，说白了就是一种最优状态估计算法。它能在有噪声的测量数据中，把真实状态给“猜”出来。你想想看，雷达测目标，有噪声吧？GPS定位，有漂移吧？卡尔曼滤波就是干这个的——把噪声滤掉，把真实轨迹还原出来。
3.1 卡尔曼滤波基本原理
卡尔曼滤波的核心思想，我总结成一句话：用模型预测，用测量修正。它假设系统是线性的，噪声是高斯白噪声。嗯，现实世界哪有那么完美？但这就是工程——在理想假设下先解决问题，再慢慢加补偿。
它的工作流程，其实就两个步骤来回循环：
- 预测（Predict）：根据上一时刻的状态，用运动模型推算出当前时刻的状态
- 更新（Update）：用当前时刻的测量值，修正预测结果
我习惯把卡尔曼滤波比作一个“有主见的厨师”。厨师做菜前会先预估味道（预测），尝一口后（测量），再决定要不要加盐（更新）。这个“加多少盐”的决策，就是卡尔曼增益在起作用。
核心公式（离散时间线性系统）：
状态方程：xk = A·xk-1 + B·uk-1 + wk-1
观测方程：zk = H·xk + vk
其中 w ~ N(0, Q)，v ~ N(0, R)
这里A是状态转移矩阵，H是观测矩阵。Q和R分别是过程噪声和测量噪声的协方差矩阵。这两个矩阵怎么设？我当年踩过坑——Q设太小，滤波器反应迟钝；Q设太大，又容易跟着噪声乱跳。后面我会细说。
3.2 状态预测与更新
咱们一步步来。先看预测阶段：
// 状态预测
x_pred = A * x_prev + B * u
// 协方差预测
P_pred = A * P_prev * A^T + Q这里x_pred是预测的状态，P_pred是预测的协方差矩阵。协方差矩阵代表什么？代表我们对预测结果的不确定度。预测越不准，P_pred就越大。
再看更新阶段：
// 卡尔曼增益计算
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我个人习惯把更新阶段叫做“纠偏”。你看，z - H*x_pred 就是“测量值减去预测值”，这叫新息（Innovation）。新息越大，说明预测和测量差得越远，就需要狠狠修正一下。
小技巧：我在做多传感器融合时，经常把不同传感器的测量值依次送入更新步骤。比如先更新雷达数据，再更新摄像头数据。这样能充分利用每个传感器的信息，而且计算量可控。
3.3 卡尔曼增益的决策意义
卡尔曼增益K，是整个滤波器的“大脑”。它决定了：你更相信模型预测，还是更相信测量数据？
K的取值在0到1之间（对于标量情况）：
- K → 0：更相信预测。测量噪声很大时，K会变小，滤波器懒得理测量值
- K → 1：更相信测量。模型不准时，K会变大，滤波器紧跟测量数据
我曾经在项目中遇到过一个问题：目标突然急转弯，卡尔曼滤波跟丢了。为什么？因为过程噪声Q设得太小，导致K也小，滤波器太“固执”，不相信测量值的大变化。后来我把Q适当调大，让K能动态变大，跟踪就稳了。
说白了，卡尔曼增益就是一个动态权重。它根据当前的不确定度，自动调整预测和测量的信任比例。这就是“决策”在滤波中的体现——每一步都在做最优决策。
注意：卡尔曼增益的计算依赖于P_pred和R。如果R设得不对（比如实际噪声比设的大），K就会偏大，导致滤波器过度相信噪声数据。我曾经吃过这个亏——在强干扰环境下，滤波器直接发散。后来我学会了在线估计R，才解决了问题。
3.4 协方差矩阵的作用
协方差矩阵P，很多人觉得它就是个中间变量，算完就扔。其实不然。P矩阵承载了状态估计的不确定度信息，是整个滤波器的“信心指标”。
P矩阵的对角线元素，代表每个状态变量的方差。比如位置方差大，说明我们对位置估计没信心；速度方差小，说明速度估计比较准。
非对角线元素呢？代表状态变量之间的相关性。比如位置和速度通常是正相关的——位置变化大时，速度往往也大。这个相关性在目标跟踪中很有用，能帮我们做更合理的预测。
| 协方差矩阵元素 | 物理含义 | 工程意义 |
|---|---|---|
| P11 | 位置估计的方差 | 位置不确定度，越大越不可信 |
| P22 | 速度估计的方差 | 速度不确定度，影响预测精度 |
| P12 = P21 | 位置与速度的协方差 | 两者相关性，用于平滑估计 |
我建议你在调试卡尔曼滤波时，把P矩阵的迹（对角线之和）画出来。如果迹突然变大，说明滤波器可能发散，或者目标发生了机动。这是一个很实用的监控指标。
另外，P矩阵的初始值怎么设？我一般设为单位矩阵乘以一个较大的数，比如P0 = 100·I。这样初始阶段滤波器会快速收敛。等稳定后，P矩阵会自然收敛到一个稳态值。
核心总结：
- 卡尔曼滤波 = 预测 + 更新，循环往复
- 卡尔曼增益K = 动态决策权重，平衡预测与测量
- 协方差矩阵P = 不确定度度量，指导滤波器行为
- Q和R的设定，直接影响滤波效果——这是调参的关键
最后说一句，卡尔曼滤波虽然经典，但它的线性假设在现实中往往不成立。后面我们会讲到扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF），它们能处理非线性系统。不过，先把线性卡尔曼吃透，后面的路就好走了。
可可精品资料库