3. 卡尔曼滤波回顾：从KF到EKF的演进
聊EKF和ESKF之前，咱们得先把老本行——卡尔曼滤波——理清楚。很多同学一上来就怼着EKF公式看，结果越看越懵。我当年也是这么过来的，后来发现，搞懂KF到EKF的演进，其实就是搞懂一件事：当系统不再听话（非线性）时，我们该怎么办。
3.1 经典KF：线性世界里的最优解
经典卡尔曼滤波，说白了就是一套递归算法。它假设你的系统是线性的，噪声是高斯白噪声。嗯，这个假设很完美，但现实世界哪有这么听话？
KF的核心思想就两个步骤：预测和更新。我习惯这么记：
- 预测：根据上一时刻的状态，猜一下当前时刻的状态长什么样。
- 更新：拿实际测量值，修正这个猜测。
公式我就不全列了，但有个关键点必须说——卡尔曼增益K。它决定了你更相信模型预测，还是更相信传感器测量。我在做IMU姿态估计时，就吃过这个亏：增益调得太大，测量噪声直接炸了；调得太小，系统又跟不上动态变化。
重要结论：KF只适用于线性系统。如果你的状态方程或观测方程里出现了x²、sin(x)、或者任何非线性项，经典KF就失效了。
3.2 非线性问题：为什么KF搞不定？
你想想看，真实世界哪有那么多线性关系？
- 机器人运动：角度变化是sin、cos，不是线性加减
- GPS定位：经纬度到笛卡尔坐标的转换，非线性
- 视觉SLAM：相机投影模型，更是非线性得一塌糊涂
经典KF假设状态转移和观测都是线性变换。一旦遇到非线性，高斯分布经过非线性变换后就不再是高斯分布了。嗯，这就麻烦了——因为KF的所有推导都建立在「高斯分布经过线性变换仍然是高斯分布」这个基础上。
避坑指南：我曾经在一个项目中，直接用KF处理带角度变化的系统，结果滤波结果发散得一塌糊涂。后来才发现，角度变化根本不是线性关系，KF根本hold不住。
3.3 EKF的核心思想：线性化
EKF的思路其实很朴素：既然非线性搞不定，那我就把它近似成线性的。
怎么近似？用泰勒展开。说白了，就是在当前估计值附近，用切线（一阶导数）来近似原来的非线性函数。这样，原来的非线性系统就变成了一个时变的线性系统，然后就可以套用KF的框架了。
具体来说，EKF做了两件事：
- 状态预测：直接用非线性函数f传播状态（不线性化）
- 协方差预测：用f的雅可比矩阵F来传播协方差（这里线性化了）
观测更新也是类似的套路。测量残差用实际测量值减去预测测量值，但协方差更新用的是观测函数的雅可比矩阵H。
个人经验：我建议你在写EKF代码时，先把雅可比矩阵推导清楚。很多bug都出在这里。我见过有人把sin的导数写成cos，结果滤波直接崩了。
3.4 EKF的局限性：不是万能的
EKF虽然好用，但坑也不少。我总结了几点：
- 线性化误差：泰勒展开只取了一阶项，如果系统非线性很强，误差会累积
- 雅可比计算麻烦：尤其在高维系统里，手动推导雅可比矩阵简直是噩梦
- 对初始值敏感：如果初始估计偏差太大，线性化点选得不好，滤波直接发散
我记得有一次做无人机姿态估计，EKF在剧烈机动时直接炸了。后来分析发现，是因为角度变化太快，线性化假设不成立了。嗯，这就是EKF的软肋。
3.5 从KF到EKF：一张图看懂演进
下面这张图，我画了KF到EKF的核心演进逻辑。你看一眼就明白了：
3.6 什么时候用EKF？什么时候别用？
根据我的经验，给你几个判断标准：
| 场景 | 推荐方案 | 理由 |
|---|---|---|
| 系统非线性弱 | EKF | 线性化误差小，计算量适中 |
| 系统非线性强 | UKF 或 ESKF | EKF的一阶近似误差太大 |
| 高维状态（>20维） | EKF（谨慎使用） | 雅可比矩阵计算量大，但UKF采样点更多 |
| IMU+GPS融合 | ESKF（下一章重点） | 误差状态线性化效果更好 |
一句话总结：EKF是KF到非线性世界的桥梁。它不完美，但足够实用。理解EKF的线性化思想，是后面理解ESKF的基础。
嗯，这一章就到这里。EKF的代码实现我就不贴了，因为下一章讲ESKF时，你会看到更优雅的写法。记住：EKF不是终点，它只是通往更高级滤波器的起点。