文档中心 / 数学讲义

第十二讲：EKF 扩展卡尔曼滤波

EKF（Extended Kalman Filter）是连接观测世界与控制决策的数学桥梁：它在噪声传感器数据中提炼出最优状态估计，并以协方差矩阵量化不确定性——EICPS 脊髓层的 CBF 和 STL 计算都依赖 EKF 提供的 x^\hat{x} 才能在物理世界中落地。

符号含义
xk∈Rnx_k \in \mathbb{R}^n真实状态（不可直接观测）
zk∈Rmz_k \in \mathbb{R}^m观测量（传感器读数）
f(⋅),h(⋅)f(\cdot), h(\cdot)非线性状态转移函数、观测函数
Qk,RkQ_k, R_k过程噪声协方差、观测噪声协方差
x^k∥k−1\hat{x}_{k\|k-1}先验估计（预测步之后，更新步之前）
x^k∥k\hat{x}_{k\|k}后验估计（更新步之后）
Pk∥k−1,Pk∥kP_{k\|k-1}, P_{k\|k}对应的误差协方差矩阵
KkK_k卡尔曼增益
Fk,HkF_k, H_k状态转移与观测函数的 Jacobian 矩阵

前言：理论发展沿革

1960 年，Rudolf Kálmán 发表了《A New Approach to Linear Filtering and Prediction Problems》，给出了线性高斯系统最优递推估计器的闭合解——卡尔曼滤波（KF）。其革命性在于：不存储所有历史数据，只用当前估计和协方差矩阵递推更新，计算量与时间无关。这一算法直接推动了阿波罗登月导航系统（AGC）的实现。

1960 年代末，线性假设的局限日益显现：真实系统的动力学和传感器都是非线性的。NASA JPL 的工程师们在处理弹道估计问题时，发展出扩展卡尔曼滤波（EKF）：在当前估计点对非线性函数进行泰勒展开，取一阶近似（Jacobian），将 KF 应用于线性化后的系统。EKF 成为此后数十年工程实践的主力状态估计器。

1995 年，Julier 与 Uhlmann 提出无迹卡尔曼滤波（UKF），用 Sigma 点采样替代 Jacobian，避免线性化误差。2009 年后，粒子滤波在高维非线性系统中展现优势。然而对于 EICPS 的实时嵌入式场景——低维状态空间、1 ms 控制周期、有限计算资源——EKF 仍是最优工程选择。

EICPS 的创新在于PINN-EKF 混合架构：用物理信息神经网络（PINN）学习系统的先验动力学模型，替代 EKF 中难以精确建模的 Jacobian，同时保留 EKF 的递推估计框架。这使得 EKF 能在模型不确定性较大的场景（如柔性臂变形、线缆接触力）中保持高精度。

1 贝叶斯估计的视角

EKF 的本质是递推贝叶斯估计。设系统状态 xkx_k 的先验分布为 p(xk)p(x_k)，观测 zkz_k 后，由贝叶斯公式更新：

p(xk∣z1:k)∝p(zk∣xk)  p(xk∣z1:k−1)p(x_k \mid z_{1:k}) \propto p(z_k \mid x_k)\; p(x_k \mid z_{1:k-1})

对线性高斯系统，该后验分布恰好是高斯分布，KF 给出其均值和协方差的精确递推公式。EKF 的核心假设：即使系统非线性，在当前估计附近局部线性近似足够准确，后验仍近似高斯。

2 EKF 递推方程

考虑非线性系统：

xk=f(xk−1, uk)+wk,wk∼N(0,Qk)x_k = f(x_{k-1},\, u_k) + w_k, \quad w_k \sim \mathcal{N}(0, Q_k)

zk=h(xk)+vk,vk∼N(0,Rk)z_k = h(x_k) + v_k, \quad v_k \sim \mathcal{N}(0, R_k)

预测步（Predict）：

x^k∣k−1=f(x^k−1∣k−1, uk)\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1},\, u_k)

Pk∣k−1=Fk Pk−1∣k−1 Fk⊤+QkP_{k|k-1} = F_k\, P_{k-1|k-1}\, F_k^\top + Q_k

其中 Jacobian：Fk=∂f∂x∣x^k−1∣k−1, ukF_k = \left.\dfrac{\partial f}{\partial x}\right|_{\hat{x}_{k-1|k-1},\, u_k}

更新步（Update）：

Kk=Pk∣k−1 Hk⊤(Hk Pk∣k−1 Hk⊤+Rk)−1K_k = P_{k|k-1}\, H_k^\top \bigl(H_k\, P_{k|k-1}\, H_k^\top + R_k\bigr)^{-1}

x^k∣k=x^k∣k−1+Kk(zk−h(x^k∣k−1))\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \bigl(z_k - h(\hat{x}_{k|k-1})\bigr)

Pk∣k=(I−KkHk) Pk∣k−1P_{k|k} = (I - K_k H_k)\, P_{k|k-1}

其中 Jacobian：Hk=∂h∂x∣x^k∣k−1H_k = \left.\dfrac{\partial h}{\partial x}\right|_{\hat{x}_{k|k-1}}

卡尔曼增益 KkK_k 的直觉：当 RkR_k 大（传感器噪声大）时，KkK_k 小，更相信预测；当 QkQ_k 大（模型不确定性大）时，KkK_k 大，更相信观测。

3 EKF 的工程约束与局限

线性化误差：Jacobian 在高度非线性区域失效，导致协方差低估和滤波发散。判断准则：若创新序列 zk−h(x^k∣k−1)z_k - h(\hat{x}_{k|k-1}) 不满足零均值高斯，滤波已发散。

可观性条件：EKF 能收敛的必要条件是系统在当前轨迹附近可观。检验：线性化观测矩阵的 Observability Gramian：

O=∑k=0N(Fk)⊤H⊤HFk\mathcal{O} = \sum_{k=0}^{N} (F^k)^\top H^\top H F^k

若 rank(O)<n\text{rank}(\mathcal{O}) < n，部分状态不可估计，需重新设计传感器配置。

计算复杂度：更新步主要代价在于矩阵求逆 (HPH⊤+R)−1(H P H^\top + R)^{-1}，复杂度 O(m3)O(m^3)（mm 为观测维数）。对 EICPS 的低维状态空间（n,m≤12n, m \leq 12），1 ms 内可完成多次 EKF 迭代。

4 EKF 在 EICPS 中的工程化

4.1 跨频状态重建

EICPS 的分层架构（V0.1 三层命名，V0.2 六层）工作在不同频率：Body 层传感器（编码器 1 kHz，视觉 30 Hz，力觉 500 Hz），Spine 层控制（1 kHz），Brain 层规划（1 Hz）。EKF 扮演跨频状态桥：

传感器层（异构输入）:
  - 编码器 q(t)：1 kHz，精确，低噪声
  - IMU a(t)：1 kHz，高频漂移
  - 视觉 p_cam(t)：30 Hz，延迟 33 ms
  - 力觉 τ(t)：500 Hz，噪声较大
      ↓  EKF 融合（1 kHz）
状态估计输出 x̂ = [q̂, q̂̇, p̂, τ̂_ext]
  ↓ 供 CBF-QP（1 kHz）和 STL 监控（1 kHz）
  ↓ 下采样后供 Brain 层规划（1 Hz）

零阶保持（ZOH）处理低频传感器：视觉数据 30 Hz，在 1 kHz 的 EKF 循环中，视觉更新步只在新帧到达时执行，其余步骤只做预测。ZOH 假设在两帧之间保持最后一次估计：

hvision(xk)={Cvision xk新帧到达跳过否则h_{\text{vision}}(x_k) = \begin{cases} C_{\text{vision}}\,x_k & \text{新帧到达} \\ \text{跳过} & \text{否则} \end{cases}

4.2 PINN 先验替代 Jacobian

传统 EKF 需要精确的 f(x,u)f(x, u) 来计算 Fk=∂f/∂xF_k = \partial f / \partial x。对 EICPS 中的柔性臂、线缆接触等复杂场景，解析模型精度不足。

PINN-EKF 混合方案：用物理信息神经网络学习 f^θ(x,u)\hat{f}_\theta(x, u)，在推理时用自动微分提供 Jacobian：

FkPINN=∂f^θ∂x∣x^k−1∣k−1, ukF_k^{\text{PINN}} = \left.\frac{\partial \hat{f}_\theta}{\partial x}\right|_{\hat{x}_{k-1|k-1},\, u_k}

PINN 的物理约束（能量守恒、接触力约束）内嵌于训练损失，保证学到的 f^θ\hat{f}_\theta 不违反基本物理规律：

LPINN=Ldata+λ1Lenergy+λ2Lcontact\mathcal{L}_{\text{PINN}} = \mathcal{L}_{\text{data}} + \lambda_1 \mathcal{L}_{\text{energy}} + \lambda_2 \mathcal{L}_{\text{contact}}

实验表明，PINN-EKF 在柔性连接场景的状态估计误差比纯刚体 EKF 降低 40-60%。

4.3 EvidencePack 中的估计质量报告

每次任务执行后，EKF 输出状态估计质量指标：

estimation_quality={tr(Pk∣k),    χ2=1N∑kνk⊤Sk−1νk,    divergence_flag}\text{estimation\_quality} = \left\{\text{tr}(P_{k|k}),\;\; \chi^2 = \frac{1}{N}\sum_k \nu_k^\top S_k^{-1} \nu_k,\;\; \text{divergence\_flag}\right\}

其中 νk=zk−h(x^k∣k−1)\nu_k = z_k - h(\hat{x}_{k|k-1}) 是创新序列，Sk=HkPk∣k−1Hk⊤+RkS_k = H_k P_{k|k-1} H_k^\top + R_k 是创新协方差。χ2\chi^2 统计量服从 χ2(m)\chi^2(m) 分布（若滤波健康），超出 3σ3\sigma 置信区间则设 divergence_flag。

5 与其他模块的数学联系

相关模块联系方式
CBF（第十讲）CBF-QP 使用 x^k∥k\hat{x}_{k\|k} 计算 h(x^)h(\hat{x}) 和李导数；估计误差引入 CBF 鲁棒性裕量 δ\delta
STL（第九讲）STL 监控器对 EKF 输出的估计轨迹 x^[0,T]\hat{x}_{[0,T]} 计算鲁棒度 ρ\rho
HTN（第十一讲）HTN Guard 条件（如 accessible=True）依赖 EKF 对接触状态的估计
Flow-Jump（第四讲）divergence_flag=True 时 EKF 失效，状态 xx 进入 Jump 集，触发安全模式
李群 SE(3)（第六讲）末端执行器姿态的 EKF 在 SE(3)SE(3) 上运行（使用对数映射保持流形约束）

Notebook

Open In Colab

本讲配套 Colab Notebook 包含：①线性 KF 到 EKF 的推导对比演示；②多频率传感器融合仿真（编码器 + 视觉 + 力觉）；③PINN-EKF 混合架构实现与 Jacobian 对比；④创新序列 χ2\chi^2 检验与滤波发散检测。

延伸阅读

  • Kalman, R.E. (1960). A New Approach to Linear Filtering and Prediction Problems. ASME.
  • Julier, S.J. & Uhlmann, J.K. (1997). A New Extension of the Kalman Filter to Nonlinear Systems. SPIE.
  • Raissi, M. et al. (2019). Physics-informed Neural Networks: A Deep Learning Framework for Solving Forward and Inverse Problems. JCP.