第十一讲：EKF 扩展卡尔曼滤波
EKF（Extended Kalman Filter）是连接观测世界与控制决策的数学桥梁：它在噪声传感器数据中提炼出最优状态估计 ，并以协方差矩阵量化不确定性——EICPS 脊髓层的 CBF 和 STL 计算都依赖 EKF 提供的 x ^ \hat{x} x ^
符号 含义 x k ∈ R n x_k \in \mathbb{R}^n x k ∈ R n 真实状态（不可直接观测） z k ∈ R m z_k \in \mathbb{R}^m z k ∈ R m 观测量（传感器读数） f ( ⋅ ) , h ( ⋅ ) f(\cdot), h(\cdot) f ( ⋅ ) , h ( ⋅ ) 非线性状态转移函数、观测函数 Q k , R k Q_k, R_k Q k , R k 过程噪声协方差、观测噪声协方差 x ^ k ∥ k − 1 \hat{x}_{k\|k-1} x ^ k ∥ k − 1 先验估计（预测步之后，更新步之前） x ^ k ∥ k \hat{x}_{k\|k} x ^ k ∥ k 后验估计（更新步之后） P k ∥ k − 1 , P k ∥ k P_{k\|k-1}, P_{k\|k} P k ∥ k − 1 , P k ∥ k 对应的误差协方差矩阵 K k K_k K k 卡尔曼增益 F k , H k F_k, H_k F 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 的本质是递推贝叶斯估计 。设系统状态 x k x_k x k p ( x k ) p(x_k) p ( x k ) z k z_k z k
p ( x k ∣ z 1 : k ) ∝ p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) p(x_k \mid z_{1:k}) \propto p(z_k \mid x_k)\; p(x_k \mid z_{1:k-1}) p ( x k ∣ z 1 : k ) ∝ p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 )
对线性高斯系统，该后验分布恰好是高斯分布，KF 给出其均值和协方差的精确递推公式。EKF 的核心假设：即使系统非线性，在当前估计附近局部线性近似足够准确 ，后验仍近似高斯。
2 EKF 递推方程
考虑非线性系统：
x k = f ( x k − 1 , u k ) + w k , w k ∼ N ( 0 , Q k ) x_k = f(x_{k-1},\, u_k) + w_k, \quad w_k \sim \mathcal{N}(0, Q_k) x k = f ( x k − 1 , u k ) + w k , w k ∼ N ( 0 , Q k )
z k = h ( x k ) + v k , v k ∼ N ( 0 , R k ) z_k = h(x_k) + v_k, \quad v_k \sim \mathcal{N}(0, R_k) z k = h ( x k ) + v k , v k ∼ N ( 0 , R k )
预测步（Predict） ：
x ^ k ∣ k − 1 = f ( x ^ k − 1 ∣ k − 1 , u k ) \hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1},\, u_k) x ^ k ∣ k − 1 = f ( x ^ k − 1∣ k − 1 , u k )
P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k ⊤ + Q k P_{k|k-1} = F_k\, P_{k-1|k-1}\, F_k^\top + Q_k P k ∣ k − 1 = F k P k − 1∣ k − 1 F k ⊤ + Q k
其中 Jacobian：F k = ∂ f ∂ x ∣ x ^ k − 1 ∣ k − 1 , u k F_k = \left.\dfrac{\partial f}{\partial x}\right|_{\hat{x}_{k-1|k-1},\, u_k} F k = ∂ x ∂ f x ^ k − 1∣ k − 1 , u k
更新步（Update） ：
K k = P k ∣ k − 1 H k ⊤ ( H k P k ∣ k − 1 H k ⊤ + R k ) − 1 K_k = P_{k|k-1}\, H_k^\top \bigl(H_k\, P_{k|k-1}\, H_k^\top + R_k\bigr)^{-1} K k = P k ∣ k − 1 H k ⊤ ( H k P k ∣ k − 1 H k ⊤ + R k ) − 1
x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − 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) x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − h ( x ^ k ∣ k − 1 ) )
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k)\, P_{k|k-1} P k ∣ k = ( I − K k H k ) P k ∣ k − 1
其中 Jacobian：H k = ∂ h ∂ x ∣ x ^ k ∣ k − 1 H_k = \left.\dfrac{\partial h}{\partial x}\right|_{\hat{x}_{k|k-1}} H k = ∂ x ∂ h x ^ k ∣ k − 1
卡尔曼增益 K k K_k K k ：当 R k R_k R k K k K_k K k Q k Q_k Q k K k K_k K k
3 EKF 的工程约束与局限
线性化误差 ：Jacobian 在高度非线性区域失效，导致协方差低估和滤波发散。判断准则：若创新序列 z k − h ( x ^ k ∣ k − 1 ) z_k - h(\hat{x}_{k|k-1}) z k − h ( x ^ k ∣ k − 1 )
可观性条件 ：EKF 能收敛的必要条件是系统在当前轨迹附近可观。检验：线性化观测矩阵的 Observability Gramian：
O = ∑ k = 0 N ( F k ) ⊤ H ⊤ H F k \mathcal{O} = \sum_{k=0}^{N} (F^k)^\top H^\top H F^k O = ∑ k = 0 N ( F k ) ⊤ H ⊤ H F k
若 rank ( O ) < n \text{rank}(\mathcal{O}) < n rank ( O ) < n
计算复杂度 ：更新步主要代价在于矩阵求逆 ( H P H ⊤ + R ) − 1 (H P H^\top + R)^{-1} ( H P H ⊤ + R ) − 1 O ( m 3 ) O(m^3) O ( m 3 ) m m m n , m ≤ 12 n, m \leq 12 n , m ≤ 12
4 EKF 在 EICPS 中的工程化
4.1 跨频状态重建
EICPS 的三层架构工作在不同频率：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 假设在两帧之间保持最后一次估计：
h vision ( x k ) = { C vision x k 新帧到达 跳过 否则 h_{\text{vision}}(x_k) = \begin{cases} C_{\text{vision}}\,x_k & \text{新帧到达} \\ \text{跳过} & \text{否则} \end{cases} h vision ( x k ) = { C vision x k 跳过 新帧到达 否则
4.2 PINN 先验替代 Jacobian
传统 EKF 需要精确的 f ( x , u ) f(x, u) f ( x , u ) F k = ∂ f / ∂ x F_k = \partial f / \partial x F k = ∂ f / ∂ x
PINN-EKF 混合方案 ：用物理信息神经网络学习 f ^ θ ( x , u ) \hat{f}_\theta(x, u) f ^ θ ( x , u )
F k PINN = ∂ f ^ θ ∂ x ∣ x ^ k − 1 ∣ k − 1 , u k F_k^{\text{PINN}} = \left.\frac{\partial \hat{f}_\theta}{\partial x}\right|_{\hat{x}_{k-1|k-1},\, u_k} F k PINN = ∂ x ∂ f ^ θ x ^ k − 1∣ k − 1 , u k
PINN 的物理约束（能量守恒、接触力约束）内嵌于训练损失，保证学到的 f ^ θ \hat{f}_\theta f ^ θ
L PINN = L data + λ 1 L energy + λ 2 L contact \mathcal{L}_{\text{PINN}} = \mathcal{L}_{\text{data}} + \lambda_1 \mathcal{L}_{\text{energy}} + \lambda_2 \mathcal{L}_{\text{contact}} L PINN = L data + λ 1 L energy + λ 2 L contact
实验表明，PINN-EKF 在柔性连接场景的状态估计误差比纯刚体 EKF 降低 40-60%。
4.3 EvidencePack 中的估计质量报告
每次任务执行后，EKF 输出状态估计质量指标：
estimation_quality = { tr ( P k ∣ k ) , χ 2 = 1 N ∑ k ν k ⊤ S k − 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\} estimation_quality = { tr ( P k ∣ k ) , χ 2 = N 1 ∑ k ν k ⊤ S k − 1 ν k , divergence_flag }
其中 ν k = z k − h ( x ^ k ∣ k − 1 ) \nu_k = z_k - h(\hat{x}_{k|k-1}) ν k = z k − h ( x ^ k ∣ k − 1 ) S k = H k P k ∣ k − 1 H k ⊤ + R k S_k = H_k P_{k|k-1} H_k^\top + R_k S k = H k P k ∣ k − 1 H k ⊤ + R k χ 2 \chi^2 χ 2 χ 2 ( m ) \chi^2(m) χ 2 ( m ) 3 σ 3\sigma 3 σ divergence_flag。
5 与其他模块的数学联系
相关模块 联系方式 CBF（第九讲） CBF-QP 使用 x ^ k ∥ k \hat{x}_{k\|k} x ^ k ∥ k h ( x ^ ) h(\hat{x}) h ( x ^ ) δ \delta δ STL（第八讲） STL 监控器对 EKF 输出的估计轨迹 x ^ [ 0 , T ] \hat{x}_{[0,T]} x ^ [ 0 , T ] ρ \rho ρ HTN（第十讲） HTN Guard 条件（如 accessible=True）依赖 EKF 对接触状态的估计 Flow-Jump（第三讲） divergence_flag=True 时 EKF 失效，状态 x x x 李群 SE(3)（第五讲） 末端执行器姿态的 EKF 在 S E ( 3 ) SE(3) S E ( 3 )
Notebook
本讲配套 Colab Notebook 包含：①线性 KF 到 EKF 的推导对比演示；②多频率传感器融合仿真（编码器 + 视觉 + 力觉）；③PINN-EKF 混合架构实现与 Jacobian 对比；④创新序列 χ 2 \chi^2 χ 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.
