4. 线性卡尔曼滤波KF

线性卡尔曼滤波（Linear Kalman Filter, KF）是状态估计领域最基础、也最重要的算法之一。它面向线性高斯系统，用一套递推公式在“预测”和“更新”之间循环，实时融合系统模型与传感器观测，输出统计意义下的最优状态估计。理解KF的五个核心方程，是后续掌握EKF、UKF、粒子滤波等非线性方法的前提。

一、核心原理：两个步骤、两个信息源

KF的核心思想可以概括为：用过程模型预测状态，用观测数据修正预测。系统在每个时刻都携带两个关键量：状态估计 x̂ 和估计协方差 P。前者表示“状态大概是多少”，后者表示“这个估计有多不确定”。

预测步骤（Time Update）利用系统动力学把上一时刻的状态和协方差向前传播；更新步骤（Measurement Update）利用当前时刻的观测值，对预测结果进行加权修正。这个加权系数就是卡尔曼增益 K，它决定了“更相信模型”还是“更相信传感器”。

KF的递推本质：每一步都只依赖上一时刻的估计和当前观测，不需要保存全部历史数据，因此非常适合嵌入式与实时系统。

二、预测与更新：五个方程

设系统模型为：

x_k = A x_{k-1} + B u_{k-1} + w_{k-1},  w ~ N(0, Q)
z_k = H x_k + v_k,                        v ~ N(0, R)

其中 A 是状态转移矩阵，B 是控制输入矩阵，H 是观测矩阵，Q 是过程噪声协方差，R 是观测噪声协方差。KF的完整递推如下：

  1. 预测状态：x̂_k⁻ = A x̂_{k-1} + B u_{k-1}
  2. 预测协方差：P_k⁻ = A P_{k-1} Aᵀ + Q
  3. 计算增益：K_k = P_k⁻ Hᵀ (H P_k⁻ Hᵀ + R)⁻¹
  4. 更新状态：x̂_k = x̂_k⁻ + K_k (z_k − H x̂_k⁻)
  5. 更新协方差：P_k = (I − K_k H) P_k⁻

第4步中的 z_k − H x̂_k⁻ 称为新息（innovation），它表示“观测值与预测观测值之间的差异”。新息越大，说明模型预测偏离实际观测越远，增益会相应地把估计拉向观测。

三、卡尔曼增益：置信度的天平

卡尔曼增益 K 是KF的灵魂。它由预测协方差 P⁻ 和观测噪声协方差 R 共同决定：

  • 当 P⁻ 很大、R 很小时，说明模型预测很不确定，而传感器很准，此时 K 接近 H⁻¹，估计主要相信观测。
  • 当 P⁻ 很小、R 很大时，说明模型预测很可信，而传感器噪声大，此时 K 接近 0，估计主要相信模型。
  • 当 P⁻ 与 R 量级相当时，K 在两者之间折中，实现最小方差融合。

工程上调参时，Q 增大意味着“更不相信模型”，R 增大意味着“更不相信传感器”。两者比例决定滤波器的动态响应速度与平滑程度。

四、协方差传播：不确定性的演化

协方差 P 描述估计误差的分布范围。预测步骤中，P_k⁻ = A P_{k-1} Aᵀ + Q 表示不确定性一方面被系统动力学放大或旋转，另一方面被过程噪声 Q 注入新的不确定性。更新步骤中，P_k = (I − K_k H) P_k⁻ 表示观测信息到来后，不确定性被压缩。

可以这样理解：预测让不确定性变大，更新让不确定性变小。在系统稳定且 Q、R 恒定时，P 会逐渐收敛到稳态值，卡尔曼增益也随之趋于常数，这就是稳态卡尔曼滤波器。

线性卡尔曼滤波：预测—更新递推循环 预测步骤（Time Update） x̂ₖ⁻ = A x̂ₖ₋₁ + B uₖ₋₁ Pₖ⁻ = A Pₖ₋₁ Aᵀ + Q 不确定性被放大并注入过程噪声 Q 更新步骤（Measurement Update） Kₖ = Pₖ⁻ Hᵀ (H Pₖ⁻ Hᵀ + R)⁻¹ x̂ₖ = x̂ₖ⁻ + Kₖ (zₖ − H x̂ₖ⁻) Pₖ = (I − Kₖ H) Pₖ⁻ 先验 x̂⁻, P⁻ 后验 x̂ₖ, Pₖ 作为下一时刻先验，递推循环 传感器观测 zₖ zₖ = H xₖ + vₖ, v ~ N(0, R) 新息 zₖ − H x̂ₖ⁻ 关键参数 Q：过程噪声协方差　R：观测噪声协方差 K：卡尔曼增益，权衡模型与观测

五、最优估计性质：为什么KF是“最优”的

在线性高斯假设下，卡尔曼滤波具有以下重要性质：

性质 含义 工程意义
最小均方误差 在所有线性估计器中，KF使估计误差的均方误差最小 给定模型和噪声统计，KF是线性最优解
无偏性 若初始估计无偏，则后续估计保持无偏 长期运行不会产生系统性偏差
一致性 估计协方差 P 与实际误差统计一致 P 可作为置信区间和故障检测依据
递推性 只需上一时刻状态和当前观测 内存占用小，适合实时嵌入式实现
稳态收敛 Q、R 恒定时，P 和 K 趋于常数 可离线计算稳态增益，降低在线计算量

六、案例场景：机器人一维位置估计

假设机器人沿直线运动，状态为位置和速度 x = [p, v]ᵀ，采样周期 dt = 0.1s。状态转移矩阵和观测矩阵为：

A = [[1, dt],
     [0,  1]]
H = [[1, 0]]        # 只观测位置
Q = diag([0.01, 0.04])
R = [[0.25]]

初始估计 x̂₀ = [0, 0]ᵀ，P₀ = diag([1, 1])。每来一次位置观测，KF先预测位置和速度，再根据增益修正。若传感器噪声较大，K 会较小，速度估计主要靠模型平滑；若传感器很准，K 会较大，位置估计快速跟随观测。

常见误区：把 Q 和 R 当作“随便调”的魔法参数。实际上它们应尽量反映真实的过程扰动和传感器噪声统计。若 Q 设得过小，滤波器会过度自信、响应迟钝；若 R 设得过小，滤波器会过度跟随噪声，输出抖动明显。

七、复盘与工程要点

  • 先验与后验：预测得到先验 x̂⁻, P⁻，更新得到后验 x̂, P，两者交替推进。
  • 增益是核心：K 自动根据 P⁻ 与 R 的相对大小分配信任权重，无需人工干预。
  • 协方差是晴雨表：P 的收敛情况可以反映滤波器是否稳定、模型是否匹配。
  • 数值稳定性：工程实现中常采用Joseph形式 P = (I−KH)P⁻(I−KH)ᵀ + KRKᵀ，避免协方差失去对称正定性。
  • 调参顺序：先根据物理意义设定 Q、R，再通过新息序列和白噪声检验判断滤波器是否健康。

掌握KF的预测、更新、增益计算和协方差传播，就掌握了状态估计的“骨架”。后续EKF正是把这套骨架推广到非线性系统：用雅可比矩阵对非线性函数做局部线性化，再套用同样的五步递推。因此，本章的五个方程值得反复推导和代码实现，直到能够不假思索地写出每一步。

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