3. 线性卡尔曼滤波回顾

无迹卡尔曼滤波（UKF）并不是凭空出现的新算法，它是在经典线性卡尔曼滤波（KF）的框架上，用确定性采样替代雅可比矩阵线性化而发展出来的。因此，在进入 UKF 之前，必须把线性 KF 的四个核心要素彻底理清：状态空间模型、预测与更新步骤、卡尔曼增益、协方差传播与数值稳定性。只有理解了线性 KF 中“信息如何流动、不确定性如何被压缩”，才能真正看懂 UKF 中 sigma 点为什么那样取、权重为什么那样算。

一、状态空间模型：把动态系统写成两行方程

线性 KF 处理的对象是线性高斯系统，其状态空间模型由状态方程和观测方程共同描述：

x_k = F_k · x_{k-1} + B_k · u_k + w_k ,  w_k ~ N(0, Q_k)
z_k = H_k · x_k + v_k ,                  v_k ~ N(0, R_k)

其中 x_k 是 k 时刻的状态向量，F_k 是状态转移矩阵，B_k 是控制输入矩阵，u_k 是已知控制量，w_k 是过程噪声；z_k 是观测向量，H_k 是观测矩阵，v_k 是观测噪声。两个噪声都假设为零均值高斯白噪声，协方差分别为 Q_k 和 R_k。

这个模型的关键假设有三条：系统动态是线性的、噪声是高斯的、噪声在时间上不相关。正是这三条假设，使得高斯分布经过线性变换后仍然是高斯分布，从而可以用均值和协方差两个量完整描述状态的后验分布。UKF 保留了“高斯假设”，但放弃了“线性假设”，这是理解两者关系的起点。

二、预测与更新：KF 的两步循环

线性 KF 的每一次迭代都分为预测（时间更新）和更新（观测更新）两步。预测步利用状态方程把上一时刻的后验估计推进到当前时刻，得到先验估计；更新步利用当前观测对先验进行修正，得到后验估计。

  • 预测步：先传播状态均值 x̂_k⁻ = F_k x̂_{k-1} + B_k u_k，再传播协方差 P_k⁻ = F_k P_{k-1} F_kᵀ + Q_k。
  • 更新步：先计算新息 y_k = z_k − H_k x̂_k⁻，再计算新息协方差 S_k = H_k P_k⁻ H_kᵀ + R_k，然后计算卡尔曼增益 K_k = P_k⁻ H_kᵀ S_k⁻¹，最后修正均值和协方差。

更新步的均值与协方差公式为：

x̂_k = x̂_k⁻ + K_k · (z_k − H_k x̂_k⁻)
P_k = (I − K_k H_k) · P_k⁻

可以看到，预测步让不确定性“变大”（加上 Q），更新步让不确定性“变小”（观测带来信息）。KF 的整个生命周期就是“膨胀—收缩”的交替过程。

线性卡尔曼滤波：预测—更新循环与协方差传播 预测步（时间更新） x̂_k⁻ = F_k x̂_{k-1} + B_k u_k P_k⁻ = F_k P_{k-1} F_kᵀ + Q_k 不确定性增大：P 加上过程噪声 Q 更新步（观测更新） S_k = H_k P_k⁻ H_kᵀ + R_k K_k = P_k⁻ H_kᵀ S_k⁻¹ 不确定性减小：观测带来信息 先验 后验反馈：x̂_k、P_k 作为下一时刻输入 卡尔曼增益 K_k 是“信任权重”：P_k⁻ 大或 R_k 小时 K_k 大，更信观测；P_k⁻ 小或 R_k 大时 K_k 小，更信预测。 P_k = (I − K_k H_k) P_k⁻ 是协方差收缩的核心表达式

三、卡尔曼增益：预测与观测之间的“信任分配器”

卡尔曼增益 K_k 是 KF 中最具解释性的量。它决定了新息 y_k 被采纳多少。从公式 K_k = P_k⁻ H_kᵀ S_k⁻¹ 可以看出：

  • 当先验协方差 P_k⁻ 很大时，说明预测很不确定，K_k 变大，滤波器更愿意相信观测。
  • 当观测噪声协方差 R_k 很大时，S_k 变大，K_k 变小，滤波器更愿意相信预测。
  • 当 H_k 把某些状态分量映射到观测空间时，只有被观测到的方向才会获得增益修正。

因此，KF 的更新不是简单地“取平均”，而是按照预测和观测各自的不确定性进行加权。这个思想在 UKF 中完全保留，只是 P_k⁻ 和 S_k 不再由线性矩阵传播得到，而是由 sigma 点统计得到。

四、协方差传播与数值稳定性

协方差传播是 KF 的“骨架”。预测步的 P_k⁻ = F_k P_{k-1} F_kᵀ + Q_k 和更新步的 P_k = (I − K_k H_k) P_k⁻ 共同决定了估计不确定性的演化。理论上，只要 P_0 对称正定、Q_k 和 R_k 对称半正定，P_k 应始终保持对称正定。但在有限精度浮点运算中，直接使用这两个公式可能出现问题：

数值问题 产生原因 典型后果 常见对策
协方差失去对称性 矩阵乘法舍入误差累积 后续增益计算出现虚部或异常 每步强制 P = (P + Pᵀ)/2
协方差失去正定性 减法形式 (I − K H)P 相消误差 对角线出现负值，滤波发散 改用 Joseph 形式更新
条件数过大 状态量纲差异大、P 接近奇异 求逆不稳定，增益剧烈波动 平方根滤波、U-D 分解
Q 或 R 设置过小 模型过度自信 增益趋零，滤波器“僵死” 合理标定 Q、R，加入下限保护

其中 Joseph 形式更新是工程中最常用的稳定化手段：

P_k = (I − K_k H_k) P_k⁻ (I − K_k H_k)ᵀ + K_k R_k K_kᵀ

该形式在数学上与标准公式等价，但由于保持了对称正定结构，对舍入误差更鲁棒。此外，平方根滤波（如 Cholesky 分解传播）和 U-D 分解滤波把协方差分解为三角因子进行传播，从根本上避免负定问题，代价是计算量增加。

工程提示：在嵌入式或高动态场景中，建议至少采用 Joseph 形式更新，并对 P 的对角线设置极小正值下限；若状态维数较高或量纲差异显著，优先考虑平方根 UKF 实现。

五、案例场景：一维匀速运动目标跟踪

设状态为 x = [位置, 速度]ᵀ，采样周期 T = 0.1 s，状态转移矩阵为：

F = [[1, T],
     [0, 1]]
H = [[1, 0]]          # 只观测位置
Q = diag([0.01, 0.04]) # 过程噪声
R = [[0.25]]           # 观测噪声

初始 P_0 = diag([1, 1])。第一次预测后，位置和速度的方差都会因 F P Fᵀ 而增大，再叠加 Q；第一次更新时，由于只观测位置，K_k 的第一行较大、第二行较小，位置被显著修正，速度只被间接修正。随着迭代进行，P 逐渐收缩并趋于稳态，K_k 也趋于常值。这个例子说明：协方差传播不仅决定估计精度，还决定滤波器“学习速率”的变化。

六、常见误区与复盘

  • 误区一：把 KF 当成“加权平均”。KF 的权重由协方差动态决定，不是固定系数；忽略 P 的传播就无法理解增益变化。
  • 误区二：只调 Q、R 不看 P 的数值健康。若 P 已失去正定性，再调 Q、R 也只是掩盖问题，应优先检查更新形式。
  • 误区三：认为线性 KF 与 UKF 无关。UKF 的预测和更新框架与 KF 完全一致，区别仅在于用 sigma 点传播均值和协方差，因此 KF 的增益公式、协方差收缩逻辑在 UKF 中仍然成立。
  • 复盘要点：先确认状态空间模型是否线性、噪声是否近似高斯；再检查预测与更新公式是否配对；最后检查协方差是否对称正定、增益是否出现异常跳变。

掌握线性 KF 的这四个要素后，再看 UKF 就会发现：它并没有推翻 KF，而是把“线性变换传播高斯分布”替换为“确定性采样传播高斯分布”。卡尔曼增益仍然是信任分配器，协方差传播仍然是核心，数值稳定性仍然是工程落地的关键。

← 上一章 📖 返回目录 下一章 →