3. 线性卡尔曼滤波回顾
扩展卡尔曼滤波（EKF）并不是凭空出现的新算法，它是在线性卡尔曼滤波（LKF）的框架上，对非线性系统做局部线性化后得到的近似推广。因此，要真正理解 EKF 的每一步推导、每一个雅可比矩阵的来源，就必须先把线性卡尔曼滤波的五个核心要素彻底吃透：状态空间模型、预测方程、更新方程、卡尔曼增益、协方差传播与最优性条件。本节将围绕这五个要素展开，为后续 EKF 的线性化处理打下坚实基础。
一、状态空间模型：把动态系统写成两个方程
线性卡尔曼滤波处理的对象是线性高斯系统，其状态空间模型由两个方程构成：
状态方程： x_k = F_{k-1} · x_{k-1} + B_{k-1} · u_{k-1} + w_{k-1}
观测方程： z_k = H_k · x_k + v_k
其中：
x_k : k 时刻的 n 维状态向量
F : 状态转移矩阵（n×n）
B : 控制输入矩阵
u : 已知控制输入
w : 过程噪声，服从 N(0, Q)
z_k : m 维观测向量
H : 观测矩阵（m×n）
v : 观测噪声，服从 N(0, R)状态方程描述系统内部状态如何随时间演化，观测方程描述状态如何被传感器“看到”。两个噪声都假设为零均值高斯白噪声，且互不相关。这一“线性 + 高斯”的假设，是卡尔曼滤波能够给出解析最优解的根本前提。
二、预测与更新：卡尔曼滤波的两步循环
卡尔曼滤波以递推方式运行，每个时刻分为预测（时间更新）和更新（测量更新）两步。
预测步：利用状态方程把上一时刻的估计外推到当前时刻，同时把协方差也一并传播：
先验状态： x̂_k⁻ = F_{k-1} · x̂_{k-1} + B_{k-1} · u_{k-1}
先验协方差：P_k⁻ = F_{k-1} · P_{k-1} · F_{k-1}ᵀ + Q_{k-1}更新步：利用当前观测 z_k 对先验估计进行修正，得到后验估计：
卡尔曼增益：K_k = P_k⁻ · H_kᵀ · (H_k · P_k⁻ · H_kᵀ + R_k)⁻¹
后验状态： x̂_k = x̂_k⁻ + K_k · (z_k − H_k · x̂_k⁻)
后验协方差：P_k = (I − K_k · H_k) · P_k⁻其中 (z_k − H_k·x̂_k⁻) 称为新息（innovation），它衡量观测与预测之间的偏差；K_k 决定“相信观测”还是“相信模型”。
三、卡尔曼增益：最优折中的数学表达
卡尔曼增益 K_k 是整个滤波器的“灵魂”。它由先验协方差 P_k⁻、观测矩阵 H_k 和观测噪声协方差 R_k 共同决定。直观理解：
- 当 P_k⁻ 很大（模型对状态很不确定）而 R_k 较小时，K_k 趋近于 H_k⁻¹，滤波器几乎完全信任观测；
- 当 R_k 很大（传感器噪声严重）而 P_k⁻ 较小时，K_k 趋近于零，滤波器几乎忽略观测，只靠模型预测；
- 当两者相当时，K_k 给出最小均方误差意义下的最优加权。
因此，K_k 不是人为设定的参数，而是由协方差自动计算出来的“最优权重”。
四、协方差传播与最优性条件
协方差 P 的传播是卡尔曼滤波区别于普通递推最小二乘的关键。预测步中 P 通过 F·P·Fᵀ 传播，并叠加 Q；更新步中 P 通过 (I − K·H) 收缩。可以证明，在“线性 + 高斯 + 噪声白且互不相关”的条件下，卡尔曼滤波给出的 x̂_k 是最小均方误差估计，也是最大后验估计，两者等价。这就是它的最优性条件。
| 要素 | 数学表达 | 工程含义 |
|---|---|---|
| 状态方程 | x_k = F·x_{k-1} + B·u + w | 描述系统演化，w ~ N(0,Q) |
| 观测方程 | z_k = H·x_k + v | 描述传感器观测，v ~ N(0,R) |
| 预测协方差 | P_k⁻ = F·P_{k-1}·Fᵀ + Q | 不确定性随模型传播并增大 |
| 卡尔曼增益 | K = P⁻·Hᵀ·(H·P⁻·Hᵀ + R)⁻¹ | 模型与观测的最优加权 |
| 后验协方差 | P = (I − K·H)·P⁻ | 观测带来信息，不确定性收缩 |
| 最优性条件 | 线性、高斯、白噪声、互不相关 | 保证最小均方误差最优 |
五、案例场景：一维匀速运动目标跟踪
设目标做匀速直线运动，状态为位置和速度 x = [p, v]ᵀ，采样周期 T。状态转移矩阵与观测矩阵为：
F = [[1, T],
[0, 1]]
H = [1, 0] # 只观测位置
Q = diag(q_p, q_v)
R = [r] # 位置观测噪声方差初始 P_0 取较大值表示“初始位置和速度都不确定”。随着滤波递推，P 的主对角线逐渐减小，说明估计越来越“自信”；当目标突然机动时，若 Q 设得过小，P 收缩过快，滤波器会来不及响应，出现明显滞后。这正是工程中需要调 Q、R 的原因。
六、常见误区与复盘
- 误区一：把 K 当成可调参数。K 是由 P、H、R 自动算出的，调 K 本质上是调 Q 和 R。
- 误区二：忽略协方差的对称性与正定性。数值计算中 P 可能因舍入误差失去对称性，工程上常做 P = (P + Pᵀ)/2 的对称化处理。
- 误区三：认为卡尔曼滤波在任何系统都最优。最优性依赖线性、高斯、白噪声假设；一旦系统非线性，就必须转向 EKF 或其他非线性滤波方法。
- 误区四：Q、R 设成“拍脑袋”常数。Q、R 应反映真实的模型误差与传感器噪声统计特性，可通过离线辨识或自适应方法估计。