📚 课程目录第4章 EKF核心公式

4、EKF核心公式：EKF的预测方程、更新方程、雅可比矩阵的计算与作用

扩展卡尔曼滤波（Extended Kalman Filter, EKF）是解决非线性系统状态估计问题的经典方法。在Edge AI的传感器融合场景中，无论是IMU与GPS的融合、视觉里程计与轮式编码器的融合，还是毫米波雷达与摄像头的融合，系统的状态转移或观测模型几乎都不可能保持严格的线性关系。EKF的核心思想并不复杂：在每一个时间步，将非线性函数在当前状态估计值处进行一阶泰勒展开，用得到的雅可比矩阵（Jacobian Matrix）来近似线性化系统，然后套用标准卡尔曼滤波的框架完成预测与更新。理解EKF的预测方程、更新方程以及雅可比矩阵的计算与作用，是掌握传感器融合算法的分水岭。

4.1 EKF预测方程：从物理模型到先验估计

EKF的预测方程解决的是“根据当前已知状态，下一时刻系统应该处于什么状态”的问题。与标准KF不同，EKF的状态转移不再是一个固定的矩阵A，而是一个可能包含三角函数、平方项、甚至查表插值的非线性函数f(x, u)。在Edge AI设备上，这个函数通常对应着惯性导航的比力方程、车辆运动学模型（如自行车模型）或机械臂的正运动学。

预测方程包含两个核心表达式：

状态预测：x̂ₖ⁻ = f(x̂ₖ₋₁, uₖ)
协方差预测：Pₖ⁻ = Fₖ · Pₖ₋₁ · Fₖᵀ + Qₖ

其中，x̂ₖ⁻表示先验状态估计（尚未融合测量值的预测结果），uₖ是控制输入（如加速度计读数、电机转速指令），Pₖ⁻是先验协方差矩阵，Qₖ是过程噪声协方差矩阵。Fₖ就是状态转移函数f对状态向量x的雅可比矩阵，在x̂ₖ₋₁处求值：

Fₖ = ∂f/∂x | x = x̂ₖ₋₁

从工程角度看，Fₖ的物理含义是“状态向量中每一个分量对另一个分量变化的敏感程度”。例如在GPS/INS融合中，如果状态向量包含位置、速度、姿态四元数，那么Fₖ中第2行第3列的元素就表示“速度对姿态变化的偏导数”，即姿态误差如何影响速度预测。这个矩阵将非线性函数f在局部“拉直”，使得协方差能够像线性系统一样沿时间传播。

Edge AI实现提示：在嵌入式平台上，Fₖ通常不是通过符号微分解析计算，而是采用数值微分（如中心差分法）或直接使用解析推导的闭式解。解析解精度高但开发周期长；数值微分通用性强但可能引入截断误差。对于资源受限的MCU，建议预先推导好Fₖ的解析表达式，避免运行时进行符号计算。

4.2 EKF更新方程：测量残差与状态修正

更新方程解决的是“当传感器返回一个测量值zₖ时，如何用这个信息修正先验估计”。观测模型同样是非线性的，用函数h(x)表示，例如从状态向量中的位置坐标计算GPS的经纬度、从姿态四元数计算磁力计的期望输出。更新方程包含三个步骤：

卡尔曼增益：Kₖ = Pₖ⁻ · Hₖᵀ · (Hₖ · Pₖ⁻ · Hₖᵀ + Rₖ)⁻¹
状态更新：x̂ₖ = x̂ₖ⁻ + Kₖ · (zₖ - h(x̂ₖ⁻))
协方差更新：Pₖ = (I - Kₖ · Hₖ) · Pₖ⁻

Hₖ是观测函数h对状态向量x的雅可比矩阵，在x̂ₖ⁻处求值：Hₖ = ∂h/∂x | x = x̂ₖ⁻。Hₖ的物理意义是“状态量的变化如何映射到测量值的变化”。以视觉SLAM为例，如果状态是相机位姿，观测是特征点的像素坐标，那么Hₖ就是重投影误差对位姿扰动的敏感度矩阵，也就是图像雅可比。

卡尔曼增益Kₖ是预测不确定性与测量不确定性之间的“信任分配器”。当测量噪声Rₖ很小（传感器精度高）时，Kₖ会变大，滤波器更相信测量值；当过程噪声Qₖ很小（模型准确）时，Pₖ⁻变小，Kₖ相应减小，滤波器更相信预测值。在Edge AI场景中，由于传感器可能间歇性失效（如GPS信号丢失），通常会在更新前检查测量值的有效性，并动态调整Rₖ的大小。

4.3 雅可比矩阵的计算方法与工程实践

雅可比矩阵的计算是EKF实现中最容易出错、也最影响性能的环节。计算方式主要有三种：

计算方法	原理	优点	缺点	适用场景
解析法	手动推导偏导数的闭式表达式	精度最高、计算量最小	推导繁琐、易出错、代码维护难	状态维度低（<10）、模型固定
数值差分法	用 (f(x+ε)-f(x-ε))/(2ε) 近似偏导	实现简单、通用性强	存在截断误差、计算量大（需多次调用f）	模型复杂、快速原型验证
自动微分	利用链式法则在代码层面自动求导	精度高、无需手动推导	依赖特定库（如PyTorch、JAX）、内存开销大	离线仿真、深度学习辅助的EKF

在Edge AI的MCU或DSP上，推荐使用解析法。虽然前期推导成本高，但运行时只需代入数值进行矩阵乘法，计算效率最高。数值差分法在每次预测和更新时都需要多次调用非线性函数，在实时性要求高的场景（如无人机飞控，1kHz控制频率）下可能造成计算超时。

关键风险：雅可比矩阵的求值点必须正确。Fₖ应在上一时刻的后验估计x̂ₖ₋₁处求值，而Hₖ应在当前时刻的先验估计x̂ₖ⁻处求值。如果混淆了求值点，会导致线性化误差增大，甚至滤波器发散。在代码实现中，建议将“求值点”作为函数参数显式传入，避免隐式依赖全局变量。

4.4 案例拆解：基于EKF的IMU+GPS融合定位

我们以一个典型的车载导航场景来完整走一遍EKF的四个核心公式。设状态向量为x = [p_e, p_n, v_e, v_n, ψ]^T，分别代表东向位置、北向位置、东向速度、北向速度和航向角。IMU提供加速度和角速度作为控制输入u，GPS提供位置和速度测量值z。

场景约束：车辆在城市峡谷中行驶，GPS信号受多路径效应影响，噪声较大（标准差σ_pos=5m）；IMU零偏已通过前期标定补偿，但仍有残余漂移。系统要求输出1Hz的融合位置，且算法需运行在Cortex-M4内核、主频168MHz的MCU上，预算CPU占用不超过30%。

第一步：定义非线性状态转移函数f（采用恒定转率和速度模型CTRV）：

pₑ' = pₑ + (v/ω)·(sin(ψ+ω·Δt) - sin(ψ))
pₙ' = pₙ + (v/ω)·(-cos(ψ+ω·Δt) + cos(ψ))
v' = v + a·Δt
ψ' = ψ + ω·Δt

其中v = √(vₑ²+vₙ²)，ω为偏航角速度，a为纵向加速度。这里f对x的雅可比Fₖ是一个5×5矩阵，需要逐项求偏导。例如，∂pₑ'/∂ψ = (v/ω)·(cos(ψ+ω·Δt) - cos(ψ))，∂pₑ'/∂v = (1/ω)·(sin(ψ+ω·Δt) - sin(ψ))。这些表达式虽然复杂，但都是初等函数的组合，可以预先推导并固化在代码中。

第二步：定义观测函数h。GPS直接输出位置和速度，因此h(x) = [pₑ, pₙ, vₑ, vₙ]^T，这是一个线性函数，Hₖ退化为常数选择矩阵：

Hₖ = [[1,0,0,0,0],
      [0,1,0,0,0],
      [0,0,1,0,0],
      [0,0,0,1,0]]

这种情况下Hₖ不需要在线计算，但为了保持代码的通用性，仍然保留雅可比计算接口。

第三步：初始化与运行。初始位置由GPS首次定位给出，初始协方差P₀设为对角阵，位置方差10m²，速度方差1m²/s²，航向方差0.1rad²。过程噪声Qₖ根据IMU的加速度计和陀螺仪噪声密度换算得到。测量噪声Rₖ根据GPS的HDOP（水平精度因子）动态调整。

第四步：结果对比与复盘。在实测数据中，纯GPS定位误差约8m（RMS），纯IMU推算在10秒后漂移超过15m。EKF融合后，定位误差稳定在2.5m以内。但调试中发现一个典型问题：当车辆急转弯时，CTRV模型的恒定转率假设不成立，导致预测误差增大。解决方案是自适应调整过程噪声Qₖ——当检测到|ω|超过阈值时，将Qₖ中的航向噪声方差放大3倍，让滤波器更信任GPS测量。

  复盘经验：EKF的性能瓶颈往往不在公式本身，而在噪声协方差矩阵Q和R的标定。Q设置过小会导致滤波器“过于自信”于模型，在模型失配时发散；Q设置过大会导致滤波结果噪声大、响应迟钝。建议使用Allen方差分析IMU噪声，使用GPS静态采集数据统计位置噪声，并在实际路测中微调。

4.5 常见误区与总结复盘

在EKF的工程实现中，以下误区最为常见：

误区一：认为EKF是“万能”的。当非线性程度很强（如卫星姿态大角度机动）时，一阶线性化误差过大，EKF可能不收敛。此时应考虑UKF（无迹卡尔曼滤波）或粒子滤波。
误区二：忽略雅可比矩阵的求值点。Fₖ和Hₖ的求值点不同，混用会导致协方差传播错误。建议在代码注释中明确标注“Fₖ at x̂ₖ₋₁”和“Hₖ at x̂ₖ⁻”。
误区三：协方差矩阵失去对称正定性。由于数值舍入误差，Pₖ可能逐渐变得不对称。应在每次更新后执行Pₖ = (Pₖ + Pₖᵀ)/2强制对称，或使用Joseph形式的协方差更新公式提高数值稳定性。
误区四：在Edge设备上使用双精度浮点。多数MCU的FPU仅支持单精度，双精度会严重拖慢计算速度。应在仿真阶段验证单精度下的滤波精度是否满足要求。

总结来看，EKF的核心公式虽然只有五个，但每个公式背后都对应着明确的物理意义和工程约束。预测方程利用系统模型外推状态，更新方程利用测量数据修正偏差，而雅可比矩阵则是连接非线性模型与线性滤波框架的桥梁。在Edge AI应用中，开发者需要根据具体的传感器特性和算力预算，选择合适的雅可比计算方法，并对噪声协方差进行仔细标定。掌握这些核心公式及其工程细节，是构建可靠传感器融合系统的第一步。