4、EKF算法流程：状态预测、观测预测、卡尔曼增益、状态更新、协方差更新全流程
扩展卡尔曼滤波（EKF）的核心价值在于将标准卡尔曼滤波的线性假设扩展到非线性系统。工程实践中，无论是无人机的姿态解算、车辆组合导航，还是机器人SLAM，系统状态方程或观测方程几乎总是非线性的。EKF通过一阶泰勒展开对非线性函数进行线性化，从而复用卡尔曼滤波的递推框架。理解EKF的五个核心步骤——状态预测、观测预测、卡尔曼增益计算、状态更新、协方差更新——是掌握其实现与调参的基础。本章将逐一拆解每个步骤的数学含义、工程实现要点及常见陷阱，并给出一个完整的端到端案例。
4.1 状态预测：非线性系统的时间递推
核心原理：状态预测利用系统模型（状态转移函数 f）将上一时刻的最优估计 x̂ₖ₋₁ 推进到当前时刻的先验估计 x̂ₖ⁻。与标准KF不同，EKF直接使用非线性函数 f 进行状态传播，而非线性化后的矩阵。例如，在车辆运动模型中，f 可能包含速度、航向角和加速度等状态的非线性组合。同时，协方差 P 的预测必须通过雅可比矩阵 Fₖ（f 对状态求偏导）进行线性化传播，即 Pₖ⁻ = Fₖ Pₖ₋₁ Fₖᵀ + Q。这里的 Q 是过程噪声协方差，代表模型本身的不确定性。
操作流程：
- 根据物理规律或运动学模型写出状态转移函数 xₖ = f(xₖ₋₁, uₖ₋₁) + wₖ，其中 wₖ ~ N(0, Q)。
- 计算雅可比矩阵 Fₖ = ∂f/∂x，在 x̂ₖ₋₁ 处求值。若 f 为线性，则 Fₖ 退化为状态转移矩阵 A。
- 执行状态传播：x̂ₖ⁻ = f(x̂ₖ₋₁, uₖ₋₁)。
- 执行协方差传播：Pₖ⁻ = Fₖ Pₖ₋₁ Fₖᵀ + Q。
4.2 观测预测与卡尔曼增益：信息融合的权重分配
核心原理：观测预测是计算在预测状态 x̂ₖ⁻ 下，传感器应当观测到的值 ẑₖ = h(x̂ₖ⁻)。h 是观测函数，同样可能是非线性的（如从笛卡尔坐标到极坐标的转换）。卡尔曼增益 Kₖ 的推导基于最小均方误差准则，其表达式为 Kₖ = Pₖ⁻ Hₖᵀ (Hₖ Pₖ⁻ Hₖᵀ + R)⁻¹，其中 Hₖ 是 h 的雅可比矩阵，R 是测量噪声协方差。增益 Kₖ 决定了预测与测量之间的信任权重：当 R 相对较小（测量噪声低）时，Kₖ 增大，滤波器更信任测量；当 Q 相对较小（模型精确）时，Pₖ⁻ 减小，Kₖ 减小，滤波器更信任预测。
判断方法：增益 Kₖ 是一个矩阵，其对角线元素反映了各状态分量的融合权重。若某个状态分量的 K 值接近 1，说明该分量主要由测量驱动；若接近 0，则主要由模型驱动。通过观察 K 的收敛趋势，可以判断滤波器是否进入稳态。
| 场景 | Q 相对大小 | R 相对大小 | K 趋势 | 滤波行为 |
|---|---|---|---|---|
| 模型精确，传感器噪声大 | 小 | 大 | K → 0 | 主要信任预测，测量修正弱 |
| 模型粗糙，传感器精度高 | 大 | 小 | K → 1 | 主要信任测量，预测修正弱 |
| 两者均衡 | 中等 | 中等 | K ≈ 0.5 | 预测与测量折中 |
4.3 状态更新与协方差更新：校正与不确定性收缩
核心原理：状态更新利用实际测量 zₖ 与观测预测 ẑₖ 之间的差异（新息，innovation）来修正先验状态：x̂ₖ = x̂ₖ⁻ + Kₖ (zₖ − h(x̂ₖ⁻))。新息向量包含了模型未能预测到的信息，Kₖ 将其按比例映射到状态修正量上。协方差更新则采用 Joseph 形式或简化形式 Pₖ = (I − Kₖ Hₖ) Pₖ⁻，表示在获得测量后，状态不确定性应当收缩。协方差更新是EKF保证估计质量的关键，若 Pₖ 不收缩或发散，说明滤波异常。
操作流程：
- 计算新息：νₖ = zₖ − h(x̂ₖ⁻)。注意新息应服从零均值高斯分布，若均值非零则可能存在系统偏差。
- 计算状态修正：Δx = Kₖ νₖ。
- 更新状态：x̂ₖ = x̂ₖ⁻ + Δx。
- 更新协方差：Pₖ = (I − Kₖ Hₖ) Pₖ⁻。工程上推荐使用 Joseph 形式 Pₖ = (I − Kₖ Hₖ) Pₖ⁻ (I − Kₖ Hₖ)ᵀ + Kₖ R Kₖᵀ，以保证数值对称性和正定性。
4.4 案例拆解：GPS/IMU 融合导航中的EKF全流程
案例背景：某地面机器人搭载低成本IMU（加速度计+陀螺仪）和单频GPS接收机，需要输出平滑且无漂移的位置、速度和姿态估计。IMU 更新频率 100Hz，GPS 更新频率 5Hz。系统状态向量为 x = [pₓ, pᵧ, vₓ, vᵧ, ψ]ᵀ（位置、速度、航向角）。状态转移函数 f 采用恒定速度模型，观测函数 h 直接输出 GPS 的位置测量。
初始现象与目标：单独使用 IMU 积分，位置漂移严重（10秒内漂移超过2米）；单独使用 GPS，输出噪声大且更新率低。目标是通过EKF融合，实现位置误差小于0.3米（RMS），且输出平滑。
关键输入数据与参数：
- 过程噪声 Q = diag(0.01, 0.01, 0.04, 0.04, 0.01)（位置噪声较小，速度噪声较大，航向噪声中等）。
- 测量噪声 R = diag(0.25, 0.25)（GPS 位置标准差约 0.5 米）。
- 初始协方差 P₀ = diag(1, 1, 1, 1, 0.1)。
分析判断过程：在 GPS 更新时刻（每 0.2 秒），执行完整的五步流程；在 IMU 更新时刻（每 0.01 秒），仅执行状态预测和协方差预测（因为没有新的测量）。这样既保证了高频输出，又利用 GPS 低频校正漂移。
实际操作步骤：
// 伪代码示意
for each time step:
// 1. 状态预测
x_pred = f(x_est, u_imu)
F = jacobian_f(x_est, u_imu)
P_pred = F * P_est * F' + Q
if GPS_measurement_available:
// 2. 观测预测
z_pred = h(x_pred)
H = jacobian_h(x_pred)
// 3. 卡尔曼增益
S = H * P_pred * H' + R
K = P_pred * H' * inv(S)
// 4. 状态更新
innovation = z_gps - z_pred
x_est = x_pred + K * innovation
// 5. 协方差更新（Joseph形式）
I_KH = eye(5) - K * H
P_est = I_KH * P_pred * I_KH' + K * R * K'
else:
x_est = x_pred
P_est = P_pred结果对比：融合后位置误差 RMS 为 0.22 米，相比纯 IMU 积分（2.1 米）和纯 GPS（0.8 米噪声）均有显著提升。输出轨迹平滑，无跳变。航向角估计在静止时仍有轻微漂移，但通过后续加入磁力计观测可进一步抑制。
风险与代价：EKF 的线性化误差在强非线性场景（如大角度机动）下可能导致滤波发散。本案例中，机器人转弯时航向角变化率较大，若 Q 中航向噪声设置过小，会出现估计滞后。此外，GPS 信号丢失时，滤波器退化为纯预测模式，位置误差随时间累积，需要设计异常检测机制。
复盘经验：调参顺序应遵循“先调 R，再调 Q，最后调 P₀”。R 通常由传感器手册或 Allan 方差分析获得；Q 需要根据实际运动动态调整，例如静止时 Q 可减小，运动时 Q 适当增大。P₀ 只影响收敛速度，不影响稳态精度。
4.5 常见误区与总结复盘
误区一：忽略雅可比矩阵的更新时机。 Fₖ 和 Hₖ 必须在当前估计点重新计算，不能复用上一时刻的值。在强非线性系统中，过时的雅可比会导致滤波精度下降甚至发散。
误区二：协方差矩阵失去正定性。 由于数值舍入误差，P 可能变成非对称或非正定矩阵。解决方法是使用 Joseph 形式更新，并定期检查 P 的特征值是否为正。
误区三：新息序列未做白噪声检验。 如果新息 νₖ 的均值明显非零或自相关较强，说明模型或噪声参数设置不当。应通过新息序列的均值和协方差来诊断滤波器健康状态。
误区四：Q 和 R 的物理意义混淆。 Q 描述的是模型误差，R 描述的是传感器误差。两者不可随意互换。若将 R 设置过小，滤波器会过度跟随测量噪声，导致输出抖动。