Home / 机器人经典基础 / Kalman滤波：把过去的观测压缩成可递推的信念

002｜Kalman滤波：把过去的观测压缩成可递推的信念

系列首页 · 上一篇 · 下一篇

原论文：R. E. Kalman，A New Approach to Linear Filtering and Prediction Problems，1960，Journal of Basic Engineering 82:35–45。原论文全文。以下公式用现代离散状态空间记号；原文的连续时间关联与控制对偶性不被等同为后来的全部滤波变体。

先接住前作的问题

D-H把关节变量变成理想位姿，却不估计输入与模型误差。这是教学上的接口联系，不是Kalman滤波从D-H推导而来。Kalman面对的是Wiener式滤波的历史信息组织问题：怎样用当前状态和误差统计继续更新，而不反复处理全部旧观测？

五个前置概念

五个前置概念是：状态空间描述随时间演化；协方差描述误差方向与相关性；条件期望利用已有信息；正交投影刻画线性最小均方解；可观测性决定哪些隐藏量可以被数据区分。

一、问题设定：历史观测怎样压缩为可递推的状态信念

机器人轮速计容易累积误差，外部定位又带噪声。应该相信哪一个？Kalman滤波不固定规定“各信一半”，而根据预测和测量的不确定性，逐步计算融合权重。它同时递推状态估计与误差协方差，使越来越长的历史不必全部保存为原始测量。

从观测到状态：首先明确隐藏量

设机器人在直线上移动，状态可取位置与速度，测量只有位置。速度虽然没有直接传感器，也可以通过时间上连续的位置变化被推断。状态不是“当前看到的数据”，而是足以描述未来演化的内部变量。

xk+1=Fkxk+Bkuk+wk,zk=Hkxk+vk.x_{k+1}=F_kx_k+B_ku_k+w_k,\qquad z_k=H_kx_k+v_k.

这里 uku_k 是已知输入，wkw_k 表示模型未描述的扰动，vkv_k 表示测量噪声；其协方差为 Qk,RkQ_k,R_k。最简单推导要求相关噪声满足相应零均值、不相关条件。若噪声相关，需要显式改变公式，不能只把它们名字写进配置文件。

要素 含义
输入 上一步估计、控制量、当前测量、模型矩阵与协方差
输出 状态估计及其误差协方差
目标 最小化估计误差的二阶量；高斯条件下对应条件均值
理论前提 线性模型与正确的统计描述；矩阵运算可定义
机器人关系 定位、传感器融合与后续EKF/SLAM的基础

二、从误差正交性推导预测与创新增益

预测与校正为什么能够分开

这里 x^k\hat x_k 是状态估计，PkP_k 是估计误差的协方差；上标 −- 表示吸收当前测量前的预测，上标 ++ 表示校正后的结果。下式省略了模型矩阵的时刻下标。预测先回答“如果暂时没有新观测，状态应该在哪里”：

x^k−=Fx^k−1++Bu,Pk−=FPk−1+F⊤+Q.\hat x^-_k=F\hat x^+_{k-1}+Bu,\qquad P^-_k=FP^+_{k-1}F^\top+Q.

状态按照动力学传播，不确定性也要一起传播。过程噪声使预测不会因为反复计算就越来越确定。

观测到来后，创新为 rk=zk−Hx^k−r_k=z_k-H\hat x^-_k。它是“实际测量减预测测量”，所以两者必须在同一测量空间。令 SkS_k 表示创新的协方差，KkK_k 表示把测量残差映回状态修正的增益。增益与校正为：

Sk=HPk−H⊤+R,Kk=Pk−H⊤Sk−1,x^k+=x^k−+Kkrk.S_k=HP^-_kH^\top+R,\quad K_k=P^-_kH^\top S_k^{-1},\quad \hat x^+_k=\hat x^-_k+K_kr_k.

还可以用单位检查增益：若标量状态位置用米、测量用厘米，则 H=100H=100，RR 与 SS 用平方厘米，KK 的单位是米/厘米，故 KrKr 才回到米。把测量单位换了却不同时变换 HH 和 RR，就是换了模型。Särkkä §4.3将状态分布和预测测量分布分开书写，这里用同样层次检查正文公式。

增益不是每个传感器固定的可信分数。它还取决于状态间相关性与测量几何，例如一次位置观测可以通过交叉协方差修正速度。

增益可以从一个很短的最优化推出来。记先验误差 e−=x−x^−e^-=x-\hat x^-，采用校正 x^+=x^−+K(z−Hx^−)\hat x^+=\hat x^-+K(z-H\hat x^-) 后，有 e+=(I−KH)e−−Kve^+=(I-KH)e^- -Kv。若 E[e−v⊤]=0E[e^-v^\top]=0，将误差外积取期望，再对 tr⁡P+\operatorname{tr}P^+ 关于 KK 求导，就得到

∂tr⁡P+∂K=2K(HP−H⊤+R)−2P−H⊤=0.\frac{\partial\operatorname{tr}P^+}{\partial K} =2K(HP^-H^\top+R)-2P^-H^\top=0.

于是增益是一个二阶统计量决定的线性最小均方解。高斯性不是这一步求导的必要条件；它额外保证完整后验仍能由均值和协方差表示，并使这个线性解等于条件均值。若 C=E[e−v⊤]≠0C=E[e^-v^\top]\ne0，则 Cov⁡(e−,r)=P−H⊤+C\operatorname{Cov}(e^-,r)=P^-H^\top+C，创新协方差也要加上 HC+C⊤H⊤HC+C^\top H^\top。只修改增益分子而继续使用原来的 SS 同样错误。

原创教学反例：先验方差为1，一次独立观测的方差也为1，更新后为 1/21/2。把同一条观测复制后当第二条独立观测，程序会报告 1/31/3，但没有增加任何信息。真正第二次观测若只是第一次的副本，其与现有估计误差已经相关。这个错误正是空间不确定性必须保留交叉协方差的原因，后者直接继承线性估计与一阶误差传播思想。

以下回到 C=E[e−v⊤]=0C=E[e^-v^\top]=0 的先验误差与测量噪声不相关情形，下一节伪代码也使用此假设。现代数值实现可用Joseph形式维护协方差：

P+=(I−KH)P−(I−KH)⊤+KRK⊤.P^+=(I-KH)P^-(I-KH)^\top+KRK^\top.

这是教学实现建议，不将其作为原论文独创结论。求增益时也通常解线性方程而不是显式构造矩阵逆。

增益把状态相关性转成修正权重；实现时还必须维护协方差。下面先用标量融合检查权重，再用重复观测图暴露把同一信息算两次的错误。

三、实现协方差更新，检查标量融合的权重

输入：x估计、P、动力学F/B/Q、测量模型H/R、u和z
预测 x_minus = F*x + B*u
预测 P_minus = F*P*F转置 + Q
计算 innovation = z - H*x_minus
计算 S = H*P_minus*H转置 + R
通过线性求解得到 K = P_minus*H转置*S的逆
校正 x = x_minus + K*innovation
用Joseph形式校正P，检查对称性与数值正定性
输出x、P及创新统计

教学例子：预测与测量各信多少

考虑标量位置估计，预测为10米，预测方差4，观测为12米，测量方差1，则 K=4/(4+1)=0.8K=4/(4+1)=0.8，更新位置为11.6米，后验方差为0.8。结果靠近测量，因为测量相对更可靠。

若把测量方差改为100，增益约0.0385，更新位置约10.077米。算法没有说“新观测一定更好”，而是按不确定性决定修正幅度。

再考虑隐藏的系统偏差：定位器总是偏移2米，但模型仍假设零均值小噪声。滤波器可以非常稳定地收敛到错误位置，并报告很小方差。平滑曲线和小协方差不能证明真实准确；必须比较真实误差与预测不确定性的一致性。

四、用重复观测图辨认虚假信息，再读原文定理

Kalman：重复观测怎样制造虚假的确定性；柱长是本文手算的方差，不是采样实验
原创教学图：本文手算与几何示意；核对相关原文。非原论文图，非实验数据。

读图：柱长是本文手算的方差，不是采样实验。一次独立观测把 1 变为 1/2；把副本当独立数据才会错误地报告 1/3。复制数据没有带来对应的真实信息增加。

原文的线性最优结论依赖二阶统计模型；把它提升为所有估计器中的最优，还需对应的分布与损失条件。

引言（PDF 第 1–2 页）的关键转向，是把无限增长的观测历史压缩成状态及其误差统计；因此应比较递推所需的信息，而非把论文读成硬件速度排行榜。

最优估计与正交投影（PDF 第 3–4 页）说明两层不同结论：合适损失与分布条件下，条件均值给出最优估计；在线性估计空间内，最优误差应与已有测量正交。后者解释为什么下一次更新只需处理不能由旧信息解释的创新。

Theorem 3 的递推与协方差方程（PDF 第 7 页）把这一结构写成可计算递推。读公式时同时跟踪状态估计和误差协方差，才能理解增益来自不确定性，而非任意固定权重。预测例子（PDF 第 9 页）展示具体随机过程的用法，不能外推为任意非线性机器人系统的保证。

原文明确区分已有概率论材料与 Theorems 3、4 的新结果；条件期望、协方差及全部线性代数不都由本篇首次提出。

这里没有训练数据规模、GPU数量或随机种子竞赛。若要评价现代实现，应另行设计重复仿真与真实传感器试验，不能把自构实验结果冒充原文。

五、检查统计失配、多峰后验与不可观测方向

“最优”始终带条件。在线性、已知二阶统计设定下可以讨论最优线性估计；联合高斯进一步使条件均值为线性，才得到更强的全体估计器比较。非高斯多峰后验用均值与协方差表示可能掩盖重要结构，例如机器人可能位于两个相似走廊中的任何一个。

非线性系统中的EKF需要线性化，UKF使用不同近似，这些不是直接套用1960年定理就自动得到全局保证。可观测性也不能由滤波器创造：如果传感器和运动从未提供某个方向的信息，调整Q与R不能凭空确定该量。

可用创新检验模型是否合理：创新不应长期带偏，时间相关性也不应与所设统计模型明显冲突。检测异常后，应检查同步、坐标和噪声模型，而不是只把R调大直到图像好看。

六、追问：同一观测复制两份后，怎样设计检验才能暴露重复计数？

同一观测复制两份后，怎样设计检验才能暴露重复计数？

参考答案：固定先验和唯一一次真实测量，先更新一次，再把完全相同的数值作为副本送入。按正文标量设定，第一次方差从1变为1/2；若第二次变为1/3，而真实数据来源没有增加，就暴露了独立性假设被重复使用。测试应比较真实误差与报告方差，不能只检查输出均值是否更平滑；相关测量必须连同交叉项建模。

七、把协方差传入空间关系，或改用多峰表示

八、自测与参考答案

问题1：没有直接测量速度，为什么位置观测有时还能更新速度？

参考答案：动力学让位置与速度误差相关，P的交叉项把位置创新传给速度分量。如果模型或运动没有建立足够信息，这种更新也可能无法可靠识别速度；能写出增益公式不等于可观测。

问题2：把R设得很小，会让估计更准确吗？

参考答案：它让算法更相信测量，却不改变测量本身的真实误差。如果R小于实际噪声或忽略偏差，估计可能抖动并过度自信。应由标定和创新统计支持噪声设定。

问题3：滤波输出更平滑，是否足以证明改进？

参考答案：不足。过度平滑可能产生滞后，快速转弯时反而偏差更大。应同时评估真实误差、响应延迟以及误差和协方差的一致性，并保持相同测量输入与评估区间。