从零推导一维卡尔曼滤波
卡尔曼滤波的公式看上去很密，但一维情况下的每一个式子，都可以从”两个不太准的数，该怎么合成一个更准的数”这个问题出发，一步步推出来。
本文不先给公式，而是沿着一串问题往下走。每一步都先凭直觉回答，再用极端情况或代码检验。最后得到的，就是一维卡尔曼滤波的全部公式。
只讨论一维：状态只有”距离”一个量，速度视为已知。速度如何一起估计，留到下一篇。
场景：雷达车跟踪前车
雷达车装有毫米波雷达，正在跟踪正前方的一辆车，雷达每 50 ms 出一帧。
假设此刻已经知道：前车距离雷达车 20.0 m，正以 10 m/s 的速度向雷达车靠近。
本文约定速度 \(v\) 是距离的变化率：负值表示靠近，正值表示远离。这里 \(v = -10\) m/s。
下一帧，也就是 50 ms 之后，前车离雷达车多远？
预测：下一帧它在哪
如果前车保持速度不变，50 ms 内距离会缩短 \(10 \times 0.05 = 0.5\) m：
\[20.0 + (-10) \times 0.05 = 19.5\ \text{m}\]用运动规律把当前状态推到下一时刻，这一步叫做预测。这里用的运动规律是匀速运动：
\[\hat{x}_{\text{预测}} = \hat{x} + v\,\Delta t\]\(\hat{x}\) 上的”帽子”表示这是一个估计值，而不是真值。由于 \(v\) 带符号，无论靠近还是远离，都用同一个公式。
但 19.5 m 只是”应该在那里”，并不精确：
- 当前的 20.0 m 和 10 m/s 本身就是估计值，可能有误差。
- 前车在这 50 ms 里不一定严格匀速，可能轻踩刹车，也可能加速。
所以预测值需要用新的观测来核对。
测量：雷达说它在哪
50 ms 后，雷达的新一帧到了，测得距离 19.8 m。
测量也不精确。雷达测距带有噪声，即使目标静止不动，每帧的读数也会有些抖动。
现在手上有两个数，都不完全可靠，而且相差 0.3 m：
| 来源 | 距离 | 不准的原因 |
|---|---|---|
| 预测 | 19.5 m | 初始估计有误差，前车不一定匀速 |
| 测量 | 19.8 m | 雷达测距噪声 |
这一帧应该输出多少米？
第一直觉：取平均
最直接的做法是取平均，得到 19.65 m。
这个做法其实已经具备了卡尔曼滤波的骨架：预测 → 测量 → 融合。问题在于，平均隐含了一个选择：两边各信一半。
设想两种极端情况：
- 情况 A：雷达误差只有 ±1 cm，前车却是新手，经常突然刹车加速。
- 情况 B：雷达测距经常跳动 ±1 m，前车开着定速巡航，速度非常稳定。
显然，情况 A 应该更信雷达，情况 B 应该更信预测。取平均在两种情况下都不对。
卡尔曼滤波的核心思想就在这里：
谁的不确定性小，就更相信谁。
谁更不靠谱，就少信谁
先把加权平均换一种写法：
\[\hat{x} = \hat{x}_{\text{预测}} + K\,(z - \hat{x}_{\text{预测}})\]- \(z\) 是雷达测量值。
- 括号里是测量和预测的差，这个例子里是 0.3 m。
- \(K\) 在 0 到 1 之间，表示这个差值采纳多少。取平均就是 \(K = 0.5\)。
再用两个数描述”有多不靠谱”：
- \(P\)：预测的不确定度
- \(R\)：测量的不确定度
数字越大越不靠谱。我们希望 \(K\) 满足：
| 情况 | 期望的 \(K\) | 含义 |
|---|---|---|
| \(R = 0\)，雷达完美 | 1 | 完全相信测量 |
| \(P = 0\)，预测完美 | 0 | 完全相信预测 |
| \(P = R\) | 0.5 | 退回平均 |
\(K\) 是一个比例。看一个数字例子：\(P = 3\)，\(R = 1\)，总共 4 份不确定性，预测占 3 份，雷达占 1 份。预测占得越多，说明预测越不靠谱，越该转向测量。所以 \(K\) 应该等于预测所占的比例：
\[K = \frac{P}{P + R}\]逐条代入上表，三个条件都满足。
这里最容易弄反的是分子。\(P\)、\(R\) 表示的是不靠谱程度，和”信任”正好相反。可以用一句话检查：
预测越不靠谱，就越听测量的。
如果写成 \(K = R/(P+R)\)，代入 \(R = 0\) 会得到 \(K = 0\)：雷达完美时反而完全不信雷达。用一个已知答案的极端情况代入，是自查公式最快的办法。
融合后的不确定度
雷达每 50 ms 来一帧，下一帧还要再融合一次。到那时需要知道当前估计有多不靠谱，所以要先算出融合后的不确定度。
误差和不确定度不是一回事
| 误差 | 不确定度 | |
|---|---|---|
| 指什么 | 这一次估计离真值差多少 | 很多次估计里，误差通常有多大 |
| 例子 | 这一帧读数 19.8 m，真值 19.75 m，误差 0.05 m | 这个雷达的误差通常在 ±10 cm 左右 |
| 能否知道 | 不能，真值永远未知 | 能，可以通过标定或统计得到 |
卡尔曼滤波中的 \(P\) 和 \(R\) 都是不确定度，具体用的是方差，也就是误差平方的平均值。
推导
融合公式可以改写成两个数的加权和：
\[\hat{x} = (1-K)\,\hat{x}_{\text{预测}} + K\,z\]因为 \((1-K) + K = 1\)，两边同时减去真值 \(x\)，误差也是同样的加权和：
\[\hat{x} - x = (1-K)\,(\hat{x}_{\text{预测}} - x) + K\,(z - x)\]右边两项分别是预测误差（方差 \(P\)）和测量误差（方差 \(R\)）。两者互相独立时，加权和的方差等于系数平方乘各自的方差：
\[P_{\text{新}} = (1-K)^2\,P + K^2\,R\]这个式子对任意 \(K\) 都成立。
一个熟悉的特例：两把精度相同、误差独立的尺子各量一次，取平均。\(K = 0.5\)，\(P = R = \sigma^2\)，得到 \(\tfrac14\sigma^2 + \tfrac14\sigma^2 = \tfrac12\sigma^2\)，方差减半。所以取两次平均比只量一次更可靠，因为误差会部分抵消。
融合不一定更好
很容易由此得出”融合两份独立信息，一定比任何一份都准”。这需要一个前提：\(K\) 选得对。
取 \(P = 1\)（预测较准）、\(R = 4\)（雷达较差）：
| \(K\) | \(P_{\text{新}}\) | 和只用预测比 |
|---|---|---|
| 0 | 1.00 | 一样，等于没用雷达 |
| 0.2 | 0.80 | 更好 |
| 0.5 | 1.25 | 更差 |
| 1 | 4.00 | 差很多，等于只用雷达 |
雷达较差时取平均，结果反而不如只用预测。误差独立只保证我们能写出 \(P_{\text{新}}\) 的公式，至于结果好不好，取决于 \(K\)。
为什么这个增益是最优的
上表里 \(K = 0.2\) 最好，而 \(P/(P+R) = 1/5\) 恰好就是 0.2。这不是巧合。
\(P_{\text{新}}\) 是关于 \(K\) 的开口向上的抛物线。对 \(K\) 求导并令其为 0：
\[\frac{dP_{\text{新}}}{dK} = -2(1-K)P + 2KR = 0\]整理得：
\[KR = (1-K)P \quad\Rightarrow\quad K = \frac{P}{P+R}\]中间那个等式的含义是：\(K\) 每增大一点，测量误差的贡献增加，预测误差的贡献减少；在最优点两者正好抵消。
这一步还说明了另一件事。前面那三个条件只能排除明显错误的公式，并不能唯一确定 \(K\)。例如 \(K = P^2/(P^2+R^2)\) 也同样满足那三个条件。真正确定 \(K = P/(P+R)\) 的，是它让融合后的方差最小。卡尔曼滤波被称为最优估计，指的就是这一点。严格来说，”最优”还依赖线性模型和高斯噪声等前提。
把最优 \(K\) 代回去：
\[P_{\text{新}} = \frac{R^2 P + P^2 R}{(P+R)^2} = \frac{PR}{P+R} = (1-K)\,P\]所以教材里常见的 \(P_{\text{新}} = (1-K)P\) 不是一条独立的规律，而是一般式在最优增益下的化简结果。如果 \(K\) 取了别的值，就要用一般式计算。
检验两个极端：
- \(P = R\)：\(P_{\text{新}} = P/2\)，和两把尺子的结论一致。
- \(R = 0\)：\(P_{\text{新}} = 0\)。雷达完美时，融合结果就是真值，确实没有任何不确定性。注意 \(P\) 是融合之前预测的不确定度，它不会因为雷达好坏而改变；”完全不信预测”是靠 \(K = 1\) 实现的，而不是让 \(P\) 变成无穷大。
预测让不确定度变大
融合之后进入下一帧。测量到来之前，先用运动模型把估计往前推 50 ms。
运动模型假设前车匀速，但它实际上可能在这 50 ms 里轻微加速或减速。模型没考虑到的这部分变化会带来新的不确定性，用 \(Q\) 表示：
\[P_{\text{预测}} = P + Q\]\(Q\) 叫过程噪声，\(R\) 叫测量噪声。它们是使用卡尔曼滤波时最需要自己设定的两个参数：
| 因素 | \(Q\) 应该 |
|---|---|
| 目标经常急加速、急刹车 | 设大 |
| 目标稳定巡航 | 设小 |
| 帧周期更长，比如从 50 ms 变成 200 ms | 变大，因为预测跨度越长，偏离越多 |
完整的一维卡尔曼滤波
每一帧依次执行两步：
① 预测
\[\begin{aligned} \hat{x}_{\text{预测}} &= \hat{x} + v\,\Delta t \\ P_{\text{预测}} &= P + Q \end{aligned}\]② 更新
\[\begin{aligned} K &= \frac{P_{\text{预测}}}{P_{\text{预测}} + R} \\ \hat{x} &= \hat{x}_{\text{预测}} + K\,(z - \hat{x}_{\text{预测}}) \\ P &= (1-K)\,P_{\text{预测}} \end{aligned}\]预测让不确定度变大（加 \(Q\)），测量让它变小（乘 \(1-K\)）。每一帧都在这一涨一缩之间达到平衡。
每个式子的来源：
| 式子 | 来源 |
|---|---|
| \(P_{\text{预测}} = P + Q\) | 模型没考虑到的变化带来新的不确定性 |
| \(P_{\text{新}} = (1-K)^2 P + K^2 R\) | 独立误差加权求和的方差 |
| \(K = P/(P+R)\) | 让上式最小 |
| \(P_{\text{新}} = (1-K)P\) | 代入最优 \(K\) 后的化简 |
初始值从哪里来
开头假设”已经知道前车距离 20.0 m、以 10 m/s 靠近”，这是为了讨论方便直接给出的。实际系统里，滤波器在第一帧之前没有任何估计，需要初始化。
距离：雷达第一次发现目标时，手上只有这一次测量，最自然的做法是
\[\hat{x}_0 = z_0, \qquad P_0 = R\]估计值就是测量值，它的不确定度就是测量的不确定度。配套实验就是这样初始化的。
如果拿不准，也可以把 \(P_0\) 设得比较大。\(P_0\) 越大，前几帧的 \(K\) 越接近 1，滤波器会主要听测量的。几帧之后，\(P\) 会被测量不断压小，初始值的影响就逐渐消失了。反过来，把 \(P_0\) 设得过小，等于一开始就过度相信一个随手给的初值，需要更多帧才能纠正。
速度：本文的一维模型把 \(v\) 当成已知常数，这只是为了先把一维的推导讲清楚。实际系统中有两种来源：
- 直接测量：毫米波雷达可以通过多普勒效应测量目标的径向速度。
- 由滤波器估计：把速度也作为状态，从连续几帧的距离变化中推断出来。这是下一篇的内容。
工程上，雷达跟踪还会先做航迹起始，例如连续若干帧都检测到同一个目标才确认建立航迹，用这几帧来给出初始距离和速度。本文不展开。
用实验检验
配套程序模拟了这样一个场景：
- 帧周期 50 ms，雷达测距的标准差 0.3 m，即 \(R = 0.09\ \text{m}^2\)。
- 滤波器假设前车始终以 10 m/s 匀速靠近，即 \(v = -10\) m/s。
- 前 100 帧确实如此；从第 100 帧起前车刹车，靠近得越来越快，\(v\) 每秒减小 2 m/s，滤波器并不知道。
融合方差与最优增益
取 \(P = 1\)、\(R = 4\)，各生成 20 万个随机误差，直接统计融合结果的方差：
| \(K\) | 统计值 | 公式 \((1-K)^2 P + K^2 R\) |
|---|---|---|
| 0.0 | 1.002 | 1.000 |
| 0.1 | 0.850 | 0.850 |
| 0.2 | 0.799 | 0.800 |
| 0.3 | 0.848 | 0.850 |
| 0.5 | 1.247 | 1.250 |
| 1.0 | 4.000 | 4.000 |
统计值与公式吻合，最小值出现在 \(K = P/(P+R) = 0.2\)。
Q = 0 时，K 会怎样
先预测：如果认为前车绝对匀速，把 \(Q\) 设为 0，\(K\) 会如何变化？
| 帧 | \(K\)（\(Q = 0\)） | \(K\)（\(Q = 0.01\)） |
|---|---|---|
| 1 | 0.500 | 0.526 |
| 10 | 0.091 | 0.283 |
| 50 | 0.020 | 0.282 |
| 199 | 0.005 | 0.282 |
\(Q = 0\) 时，每一帧只有测量在压缩 \(P\)，没有任何东西让它涨回来，\(K\) 按 \(1/(n+1)\) 一直趋近 0。滤波器越来越自信，也越来越不听雷达的。
\(Q = 0.01\) 时，\(K\) 很快稳定在 0.28 左右，这就是一涨一缩达到的平衡，称为稳态。
前车刹车之后
估计误差（估计值减真值，单位 m）：
| 帧 | 雷达本身 | \(Q = 0\) | \(Q = 0.01\) |
|---|---|---|---|
| 90 | 0.03 | −0.01 | −0.00 |
| 120 | −0.02 | 1.07 | 0.15 |
| 160 | 0.11 | 8.23 | 0.95 |
| 199 | 0.34 | 20.97 | 1.40 |
均方根误差：
| 区间 | 雷达本身 | \(Q = 0\) | \(Q = 0.01\) |
|---|---|---|---|
| 匀速段 | 0.28 | 0.03 | 0.12 |
| 刹车段 | 0.30 | 9.70 | 0.76 |
直觉上可能会以为 \(Q = 0\) 的滤波器只是反应延迟，迟早能追上。实际上它发散了：到第 199 帧偏差达 21 m，雷达一直在给正确读数，滤波器却几乎完全不理会。
这组数据还显示了 \(Q\) 的取舍。匀速段里 \(Q = 0\) 的效果最好，因为那段时间模型恰好是对的。\(Q\) 设得小，平稳时更平滑，但目标一变化就反应迟钝甚至发散；\(Q\) 设得大，跟得快，但对噪声更敏感。
容易混淆的地方
| 混淆 | 区分 |
|---|---|
| 不确定度和信任 | 不确定度越大越不可信。\(K\) 的分子是预测的不确定度 \(P\) |
| 不确定度和误差 | 误差是某一次的具体偏差，无法得知；不确定度描述误差通常有多大，可以标定 |
| 误差独立和融合更好 | 独立让方差公式成立；只有 \(K\) 选得好，融合结果才会更好 |
| \((1-K)P\) 和一般式 | \((1-K)P\) 只在最优 \(K\) 下成立，一般情况用 \((1-K)^2P + K^2R\) |
| \(P\) 和 \(P_{\text{新}}\) | \(P\) 是融合前预测的不确定度，\(P_{\text{新}}\) 是融合后的 |
| \(Q = 0\) 和延迟 | \(Q = 0\) 会让 \(K \to 0\)，模型一旦失配，滤波器会发散，而不只是延迟 |
这个模型缺了什么
再看刹车段的结果：即使 \(Q = 0.01\)，均方根误差 0.76 m 仍然比直接用雷达读数（0.30 m）还差，而且误差在慢慢变大。
原因在预测公式 \(\hat{x}_{\text{预测}} = \hat{x} + v\,\Delta t\) 里：\(v\) 是写死的 −10 m/s。前车刹车后，真实的靠近速度越来越大，滤波器却每帧都按 −10 m/s 预测。雷达每帧把位置往回拉，下一帧预测又按错误的速度跑偏。调大 \(Q\) 只能让它更快被拉回来，不能解决根本问题。
根治的办法是让滤波器把速度也作为状态来估计。这会带来几个新问题：
- 雷达只测距离时，怎么修正从未被直接测量的速度？
- 速度的修正幅度应该由什么决定？
- 位置误差和速度误差是否有联系，怎么描述这种联系？
这些问题会引出协方差矩阵和多维卡尔曼滤波，留到下一篇。
配套程序见 examples/kalman-1d，只用 Python 标准库，随机种子固定，结果可复现。