1. 绪论：为什么需要EKF？从线性KF到非线性EKF的动机与直观理解
1.1 先聊聊我为什么对卡尔曼滤波“又爱又恨”
做机器人这么多年，我接触的第一个状态估计工具就是卡尔曼滤波。说实话，刚学的时候觉得这东西真漂亮——公式简洁，逻辑自洽，收敛速度快。但一上手实际项目，问题就来了。
我记得很清楚，2017年我在做一个室内移动机器人的定位项目。轮式里程计加上IMU，按理说用标准KF就能搞定。但机器人一转弯，模型就开始飘。为什么？因为运动方程里有个sin和cos，而标准KF只认线性关系。
嗯，这就是我们这章要聊的核心问题：现实世界是非线性的，而标准卡尔曼滤波只擅长处理线性系统。
一句话总结： 标准KF是“线性世界的优等生”，EKF是“非线性世界的实战派”。
1.2 标准KF到底在做什么？
先简单回顾一下。标准卡尔曼滤波解决的是这样一个问题：
- 你有一个系统，它的状态随时间变化
- 你能观测到一些带噪声的测量值
- 你想从这些不完美的信息中，估计出真实状态
它假设两件事是线性的：
- 状态转移方程：
x_k = A * x_{k-1} + B * u_k + w - 观测方程：
z_k = H * x_k + v
说白了，就是当前状态只跟上一状态成比例关系，观测也只跟状态成比例关系。矩阵A、H都是常数，不随状态变化。
这种情况下，高斯分布经过线性变换后还是高斯分布。KF就是利用这个性质，在“预测”和“更新”两步之间来回迭代，不断修正估计。
我的个人习惯： 每次讲KF，我都会先画一条直线，再画一堆散点。直线就是模型预测，散点就是观测数据。KF做的事，就是在这两者之间找一个“最优折中”。
1.3 非线性来了，标准KF为什么扛不住？
现在问题来了。你的机器人运动方程可能是这样的：
x_k = x_{k-1} + v * cos(θ) * dt
y_k = y_{k-1} + v * sin(θ) * dt
θ_k = θ_{k-1} + ω * dt看到cos(θ)和sin(θ)了吗？这就是非线性。θ变了，cos(θ)的变化不是线性的。你没法用一个常数矩阵A来描述这种关系。
为什么会这样？因为非线性函数会“扭曲”概率分布。一个高斯分布经过非线性变换后，就不再是高斯分布了。而KF的所有推导都建立在“高斯分布经过变换后还是高斯”这个前提上。前提不成立，结果自然不准。
我在项目中遇到过最典型的例子：用标准KF做GPS+IMU融合，车辆直线行驶时效果很好，一旦急转弯，估计的位置直接飞出道路。当时排查了半天，最后发现就是非线性项在作怪。
1.4 EKF的核心思想：用线性近似“骗过”非线性
EKF的思路其实很朴素：既然非线性不好处理，那我就找一个线性函数来近似它。
怎么近似？用泰勒展开。在当前的估计点附近，把非线性函数展开成一阶泰勒级数，只保留线性项。这样，原来的非线性函数就被一个切线（或切平面）替代了。
你想想看，这就像你在爬山时，不知道整座山的形状，但你知道脚下这一小块地方是陡是缓。用这一小块的信息来估计下一步怎么走，虽然不完美，但够用。
具体来说，EKF做了两件事：
- 预测步：用真实的非线性函数传播状态均值，但用线性化后的雅可比矩阵传播协方差
- 更新步：用真实的非线性函数计算预测观测，但用线性化后的雅可比矩阵计算卡尔曼增益
说白了，就是“均值走真实路径，协方差走近似路径”。
避坑指南： 我曾经在EKF初始化时吃过亏。如果初始状态估计偏差太大，泰勒展开的近似点离真实点很远，线性化误差会非常大，导致滤波器直接发散。所以EKF对初始值比较敏感，这一点要牢记。
1.5 一张图看懂KF到EKF的演进
下面这张SVG图，我用最直观的方式展示了从线性KF到非线性EKF的核心逻辑变化：
左边是标准KF的世界——一切都是线性的，高斯分布经过变换后还是完美的椭圆。右边是EKF的世界——非线性函数把椭圆扭曲成了奇怪的形状，EKF通过线性化强行把它“拉回”椭圆近似。
1.6 EKF的局限性：不是万能药
EKF虽然好用，但也不是银弹。我总结了几点：
| 问题 | 表现 | 我的建议 |
|---|---|---|
| 线性化误差 | 强非线性下近似精度差 | 考虑UKF或粒子滤波 |
| 雅可比计算 | 复杂系统求导容易出错 | 用符号计算或数值差分验证 |
| 初始值敏感 | 初始偏差大容易发散 | 先用其他方法粗估计 |
| 高斯假设 | 多模态分布无法处理 | 改用粒子滤波 |
我的经验： 如果你发现EKF在某个场景下频繁发散，先别急着换算法。检查一下雅可比矩阵算对了没有。我曾经花了两天时间调试一个SLAM系统，最后发现是角度求导时忘了转弧度——这种低级错误在EKF实现中非常常见。
1.7 什么时候该用EKF？
说了这么多，到底什么场景适合EKF？我个人的判断标准很简单：
- 系统弱非线性：比如角度变化不大时，sin≈θ，cos≈1，这时候EKF效果很好
- 计算资源有限：EKF的计算量比UKF和粒子滤波小得多
- 需要实时性：在嵌入式系统或机器人上，EKF是性价比最高的选择
- 你熟悉雅可比求导：如果团队里有数学功底好的人，EKF实现起来很快
反过来，如果系统非线性特别强（比如大角度机动），或者状态维度很高（比如几百维），那EKF就不太合适了。这时候可以考虑UKF或者ESKF（误差状态卡尔曼滤波）。
嗯，关于EKF的动机和直观理解，就先聊到这里。下一章我们会从数学上一步步推导EKF的公式，到时候你会看到，那些看起来复杂的雅可比矩阵，其实每一步都有清晰的物理含义。