3、扩展卡尔曼滤波的痛点：EKF的线性化过程，雅可比矩阵的计算负担与误差累积

好，咱们接着聊。上一节我讲了为什么标准卡尔曼滤波不够用，引出了EKF。但说实话，EKF在实际工程中，真的让人又爱又恨。

为什么？因为它有两个绕不开的痛点：线性化过程和雅可比矩阵的计算。这两个问题，说白了就是EKF的“阿喀琉斯之踵”。

3.1 线性化：用切线近似曲线

EKF的核心思想，是用一个线性模型去近似非线性系统。怎么近似？用泰勒展开，只取一阶项。

你想想看，一个非线性函数，比如 y = sin(x)，它在某个点附近，可以用一条切线来近似。这就是线性化。

但问题来了——切线终究是切线，它永远不等于曲线本身。

我记得有一次做室内机器人定位，用的就是EKF。机器人转弯时，运动模型是非线性的。我明明看到它转了个90度的弯，但EKF估计出来的轨迹，却像被“拉直”了一样。为什么？因为线性化把弯曲的路径近似成了一条直线，误差就这么来了。

3.2 雅可比矩阵：计算负担的“无底洞”

EKF的线性化，需要计算雅可比矩阵。雅可比矩阵是什么？说白了，就是所有偏导数的集合。

对于一个n维状态向量，雅可比矩阵的大小是 n×n。如果状态维度是10，那就是100个偏导数。如果维度是50，那就是2500个偏导数。

我曾在项目中遇到过传感器数量特别多的情况——IMU、GPS、轮式里程计、激光雷达，状态维度直接飙到了30多。每次更新，都要重新计算雅可比矩阵。那段时间，我每天都在跟偏导数打交道，代码里全是链式法则。

更让人头疼的是，雅可比矩阵的推导极易出错。稍微一个符号写反，整个滤波器就会发散。我曾经因为一个负号，排查了整整两天。

3.3 误差累积：一步错，步步错

EKF的误差累积，是一个典型的“滚雪球”效应。

第一次线性化，引入了一点误差。这个误差会进入状态估计，影响下一次的线性化点。下一次的线性化点偏了，线性化误差更大。如此循环，误差越滚越大。

我习惯用一个比喻来解释：EKF就像在走钢丝，每一步都小心翼翼，但每一步都在偏离中心线。

具体来说，误差累积体现在三个方面：

• 状态估计偏差：线性化误差导致均值估计有偏
• 协方差估计不准确：雅可比矩阵的近似导致协方差矩阵被低估或高估
• 滤波器不一致：实际误差远大于滤波器报告的协方差，导致滤波器“过于自信”

我做过一个对比实验：同一个非线性系统，用EKF和UKF分别跑100次蒙特卡洛仿真。结果EKF的均方根误差，是UKF的3倍以上。而且EKF的协方差矩阵，明显比实际误差小——它“以为自己很准”，其实已经偏了。

3.4 一个简单的例子：一维非线性系统

咱们看一个最简单的例子，你就能直观感受到EKF的问题。

假设系统模型是：

x_k = 0.5 * x_{k-1} + 0.1 * sin(x_{k-1}) + w_k
z_k = x_k^2 + v_k

这里，状态转移和观测都是非线性的。

EKF的做法：

1. 在 x_{k-1} 处对状态转移函数求导，得到雅可比 F = 0.5 + 0.1 * cos(x_{k-1})
2. 在预测状态 x_{k|k-1} 处对观测函数求导，得到雅可比 H = 2 * x_{k|k-1}
3. 用这两个雅可比矩阵，进行标准的卡尔曼滤波更新

问题出在哪？

• sin(x) 的线性化，只在 x_{k-1} 附近有效。如果状态变化大，误差就大。
• x^2 的线性化，只在 x_{k|k-1} 附近有效。如果预测不准，观测更新也会偏。

而UKF呢？它不需要任何雅可比矩阵。它用一组sigma点，直接通过非线性函数传播，然后从传播后的点中恢复出均值和协方差。说白了，UKF用“采样”代替了“求导”。

3.5 小结

EKF的痛点，总结起来就三句话：

• 线性化有误差：切线永远不等于曲线
• 雅可比计算负担大：推导繁琐，容易出错，高维度下计算量大
• 误差会累积：一步错，步步错，滤波器可能发散

嗯，这就是为什么后来出现了UKF。下一节，我会详细讲UKF是如何用“无迹变换”来避开这些痛点的。你想想看，如果有一种方法，不需要求导，不需要计算雅可比，而且精度还更高，是不是很吸引人？