卡尔曼滤波基础：线性卡尔曼滤波的数学模型、预测与更新步骤、核心公式推导
在进入扩展卡尔曼滤波（EKF）之前，必须彻底理解其前身——线性卡尔曼滤波（KF）。EKF 的本质是“将非线性系统线性化后套用 KF 框架”，因此 KF 的数学模型、预测-更新循环以及协方差传播逻辑，构成了整个课程的地基。本章将严格推导线性 KF 的核心公式，并解释每个矩阵和符号的物理含义，为后续处理非线性系统打下坚实的数学直觉。
2.1 线性系统的状态空间描述与问题定义
线性卡尔曼滤波解决的是如下问题：给定一个离散时间线性动态系统，其状态向量 xk（n 维）随时间演化，并且我们只能通过带噪声的测量 zk（m 维）间接观测它。系统的完整数学模型由两个方程定义：
- 状态方程（过程模型）：xk = A·xk-1 + B·uk + wk，其中 A 是 n×n 状态转移矩阵，B 是 n×l 控制输入矩阵，uk 是 l 维控制向量，wk 是过程噪声，通常假设为均值为零、协方差为 Q 的高斯白噪声。
- 测量方程（观测模型）：zk = H·xk + vk，其中 H 是 m×n 测量矩阵，vk 是测量噪声，均值为零、协方差为 R 的高斯白噪声。
这里的关键假设是：wk 和 vk 互不相关，且都与初始状态 x0 独立。KF 的目标是：在每一时刻 k，基于所有历史测量值 z1:k，给出状态 xk 的最优线性无偏估计，并量化估计的不确定性（即误差协方差矩阵 Pk）。
2.2 预测步骤：时间更新方程
预测步骤利用系统模型，从上一时刻的后验估计（x̂k-1, Pk-1）推算出当前时刻的先验估计（x̂k-, Pk-）。上标“-”表示这是融合测量值之前的估计。
先验状态估计直接由状态方程取期望得到：
x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ由于过程噪声 wk 均值为零，它不会影响状态的期望值，但会影响估计的不确定性。先验误差协方差的推导如下：
定义真实误差: eₖ⁻ = xₖ - x̂ₖ⁻
代入状态方程和先验估计:
eₖ⁻ = (A·xₖ₋₁ + B·uₖ + wₖ) - (A·x̂ₖ₋₁ + B·uₖ)
= A·(xₖ₋₁ - x̂ₖ₋₁) + wₖ
= A·eₖ₋₁ + wₖ
先验协方差:
Pₖ⁻ = E[eₖ⁻·(eₖ⁻)ᵀ]
= E[(A·eₖ₋₁ + wₖ)·(A·eₖ₋₁ + wₖ)ᵀ]
= A·E[eₖ₋₁·eₖ₋₁ᵀ]·Aᵀ + E[wₖ·wₖᵀ] （交叉项为零，因 eₖ₋₁ 与 wₖ 独立）
= A·Pₖ₋₁·Aᵀ + Q这个推导揭示了两个核心事实：第一，协方差通过 A 矩阵进行“相似变换”传播，A 将上一时刻的误差映射到当前时刻；第二，过程噪声 Q 直接叠加在协方差上，它代表了模型本身的不确定性——即使上一时刻估计完美，过程噪声也会引入新的不确定性。
2.3 更新步骤：测量更新方程
当获得新的测量值 zk 后，我们将其与预测值融合。融合的核心是计算卡尔曼增益 Kk，它决定了预测和测量之间的信任权重。增益的推导基于最小化后验误差协方差的迹（即最小均方误差准则）。
步骤一：计算测量残差（新息）
yₖ = zₖ - H·x̂ₖ⁻新息 yk 表示实际测量与预测测量之间的差异。如果模型准确且估计良好，新息应接近零，其协方差为 Sk = H·Pk-·HT + R。
步骤二：计算卡尔曼增益
Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹增益矩阵 Kk 的维度为 n×m。它的物理含义是：将 m 维测量空间的新息映射到 n 维状态空间的修正量。当测量噪声 R 很小（传感器精确）时，Kk 增大，滤波器更信任测量；当过程噪声 Q 很小（模型精确）时，Pk- 减小，Kk 减小，滤波器更信任预测。
步骤三：更新后验状态估计
x̂ₖ = x̂ₖ⁻ + Kₖ·yₖ步骤四：更新后验误差协方差
Pₖ = (I - Kₖ·H)·Pₖ⁻后验协方差公式的推导利用了增益的最优性。将 x̂k 代入误差定义，经过代数化简（此处省略中间步骤），最终得到上述简洁形式。值得注意的是，Pk 与测量值 zk 无关，只与 Pk-、H 和 R 有关，因此可以离线预先计算。
| 步骤 | 公式 | 维度 | 关键作用 |
|---|---|---|---|
| 先验状态 | x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ | n×1 | 利用模型外推状态 |
| 先验协方差 | Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q | n×n | 传播不确定性，叠加过程噪声 |
| 卡尔曼增益 | Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹ | n×m | 权衡预测与测量的权重 |
| 后验状态 | x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻) | n×1 | 用新息修正状态估计 |
| 后验协方差 | Pₖ = (I - Kₖ·H)·Pₖ⁻ | n×n | 降低估计不确定性 |
2.4 案例拆解：一维匀速运动目标跟踪
为了将抽象公式落地，我们考虑一个经典的一维目标跟踪问题。假设一辆小车在直线上匀速行驶，我们通过雷达测量其位置（带噪声），需要实时估计位置和速度。
系统建模：状态向量 x = [位置 p, 速度 v]T。离散时间步长 Δt = 0.1s。状态转移矩阵和测量矩阵为：
A = [[1, Δt], [0, 1]] H = [[1, 0]]过程噪声 Q 反映加速度扰动（假设小车可能有轻微加速/减速），取 Q = [[0.01, 0], [0, 0.01]]。测量噪声 R = 0.04（对应位置测量标准差 0.2m）。初始状态 x̂₀ = [0, 1]T，初始协方差 P₀ = [[1, 0], [0, 1]]。
仿真运行：我们模拟真实位置 ptrue(k) = 0.1·k（速度 1m/s），测量值 zk = ptrue(k) + N(0, 0.2²)。运行 50 步 KF 后，观察结果：
- 收敛过程：前 5 步，位置估计误差从初始的 0.5m 迅速下降到 0.15m 左右；速度估计从初始的 0 收敛到 0.95~1.05m/s 区间。
- 稳态行为：约 20 步后，位置估计误差稳定在 ±0.1m 以内，速度误差稳定在 ±0.05m/s 以内。此时卡尔曼增益 K 收敛到常数 [0.38, 0.15]T。
- 协方差演化：Pk 的对角线元素从初始的 1 逐渐下降到稳态值 P11≈0.015, P22≈0.002，表明滤波器对状态的置信度不断提高。
关键观察：如果我们将 R 调大 10 倍（R=0.4），滤波器会更平滑但响应变慢，位置误差增大到 ±0.2m；如果将 Q 调小 10 倍（Q=0.001），滤波器会过于自信，当真实目标突然加速时，估计会出现明显的滞后偏差。这直观展示了 Q 和 R 的“信任权衡”作用。
2.5 常见误区与总结复盘
线性 KF 虽然数学优雅，但在实际应用中有几个高频误区需要警惕：