3. EKF算法推导：状态预测、观测更新、卡尔曼增益计算

好，咱们直接进入正题。EKF（扩展卡尔曼滤波）说白了，就是把标准卡尔曼滤波那套东西，硬搬到非线性系统里来用。你想想看，现实世界哪有那么多线性系统？机器人轮子打滑、传感器有死区、运动模型带三角函数……全是非线性的。标准KF处理不了，怎么办？

我个人的习惯是，先别急着看公式，先理解它想干什么。EKF的核心思想就一句话：用泰勒展开把非线性函数线性化，然后套用KF的框架。就这么简单。

3.1 状态预测：先猜一步

假设我们有一个非线性系统，状态方程长这样：

x_k = f(x_{k-1}, u_k) + w_k

其中 f 是非线性函数，u_k 是控制输入，w_k 是过程噪声（高斯白噪声，协方差为 Q）。

标准KF里，状态预测就是直接乘个矩阵。但这里不行，f 不是线性的。怎么办？线性化。

我们在上一时刻的最优估计点 x_{k-1|k-1} 附近，对 f 做一阶泰勒展开：

f(x) ≈ f(x̂) + F * (x - x̂)

这里的 F 就是雅可比矩阵：

F = ∂f/∂x |_{x = x̂_{k-1|k-1}}

嗯，这里要注意。雅可比矩阵的维度是 n×n（n是状态向量维度）。我刚开始学的时候，老是把雅可比矩阵的求法搞混。后来我总结了一个笨办法：状态方程的每个输出分量，对每个状态变量求偏导。一行一个输出分量，一列一个状态变量。

有了这个线性化后的模型，状态预测就跟标准KF一样了：

x̂_{k|k-1} = f(x̂_{k-1|k-1}, u_k)
P_{k|k-1} = F * P_{k-1|k-1} * F^T + Q

注意看，状态预测值 x̂_{k|k-1} 还是用原始的非线性函数 f 算的，没有用线性近似。只有协方差 P 的传播用了雅可比矩阵 F。为什么？因为协方差描述的是不确定性，我们需要知道这个不确定性是怎么被非线性函数“扭曲”的，而一阶近似已经够用了。

3.2 观测更新：用测量值修正

观测方程也是非线性的：

z_k = h(x_k) + v_k

h 是非线性观测函数，v_k 是观测噪声（协方差为 R）。

同样的套路，在预测状态 x̂_{k|k-1} 附近对 h 做线性化：

h(x) ≈ h(x̂) + H * (x - x̂)

雅可比矩阵 H 为：

H = ∂h/∂x |_{x = x̂_{k|k-1}}

H 的维度是 m×n（m是观测向量维度）。

观测更新的第一步，计算残差（也叫新息）：

y_k = z_k - h(x̂_{k|k-1})

这个残差就是“实际测量值”和“预测的测量值”之间的差异。如果残差很大，说明预测不准，或者测量有异常。

我在项目中遇到过一件事：有一次机器人定位突然漂移，我查了半天，发现是某个传感器被遮挡了，导致残差突然变大。从那以后，我习惯在代码里加一个残差阈值检查，超过阈值就降低该传感器的权重，甚至直接丢弃。

3.3 卡尔曼增益计算：信任谁？

卡尔曼增益 K 决定了我们更相信预测还是更相信测量。它的计算公式：

K_k = P_{k|k-1} * H^T * (H * P_{k|k-1} * H^T + R)^{-1}

这个公式看着复杂，其实逻辑很直观：

• 分子：P_{k|k-1} * H^T —— 预测的不确定性，通过观测函数映射到观测空间
• 分母：H * P_{k|k-1} * H^T + R —— 预测不确定性 + 测量噪声，即总的不确定性

说白了，K 就是“预测不确定性占总不确定性的比例”。

• 如果测量噪声 R 很小（传感器很准），分母 ≈ 预测不确定性，K 接近 1，我们更相信测量
• 如果预测不确定性 P 很小（模型很准），分子很小，K 接近 0，我们更相信预测

有了 K，就可以更新状态和协方差了：

x̂_{k|k} = x̂_{k|k-1} + K_k * y_k
P_{k|k} = (I - K_k * H) * P_{k|k-1}

这里有个细节：协方差更新公式还有另一种形式（Joseph form），数值稳定性更好。我建议你在实际工程中用 Joseph form：

P_{k|k} = (I - K_k * H) * P_{k|k-1} * (I - K_k * H)^T + K_k * R * K_k^T

为什么？因为标准形式在计算机里做浮点运算时，容易因为舍入误差导致 P 失去对称正定性。我曾经被这个问题坑过——滤波器跑了几个小时突然发散，查了两天才发现是协方差矩阵变成了非正定。从那以后，我所有项目都用 Joseph form。

3.4 完整EKF算法流程

把上面三部分串起来，就是完整的EKF算法：

1. 初始化：设定初始状态 x̂₀ 和初始协方差 P₀
2. 预测步：
   • 计算雅可比矩阵 F = ∂f/∂x
   • x̂_{k|k-1} = f(x̂_{k-1|k-1}, u_k)
   • P_{k|k-1} = F * P_{k-1|k-1} * F^T + Q
3. 更新步：
   • 计算雅可比矩阵 H = ∂h/∂x
   • y_k = z_k - h(x̂_{k|k-1})
   • K_k = P_{k|k-1} * H^T * (H * P_{k|k-1} * H^T + R)^{-1}
   • x̂_{k|k} = x̂_{k|k-1} + K_k * y_k
   • P_{k|k} = (I - K_k * H) * P_{k|k-1}（或用Joseph form）
4. 重复：k = k+1，回到步骤2

你看，整个流程跟标准KF几乎一模一样，区别就在于多了两步雅可比矩阵的计算。这也是EKF最大的优点——代码框架不用大改，只需要把线性模型换成非线性模型，再加个求导函数就行。

3.5 EKF的核心逻辑图

下面这张图，是我自己画的一个EKF流程概览。你可以把它当作一个“地图”，每次写代码前看一眼，思路就清晰了。

这张图里，我特意把“计算雅可比矩阵”放在每个步骤的第一条。为什么？因为这是EKF和标准KF唯一的区别，也是最容易出错的地方。你写代码的时候，先把雅可比矩阵算对，剩下的就是套公式了。

---

公众号:蓝海资料掘金营，微信deep3321