数学基础回顾：泰勒展开、雅可比矩阵、协方差矩阵
各位同学，在正式进入扩展卡尔曼滤波（EKF）的代码实战之前，我觉得有必要先停下来，把三个数学工具好好捋一遍。为什么？因为EKF说白了就是「用线性方法去近似非线性问题」，而这三个工具正是完成这个近似的核心武器。
我个人习惯是，先理解数学直觉，再去看公式。不然你硬背公式，写代码时一调试就懵。好，我们一个一个来。
1. 泰勒展开：把弯的掰直
先问大家一个问题：现实世界中的系统，比如机器人轮子打滑、传感器噪声，这些模型是线性的吗？
答案往往是否定的。非线性才是常态。但卡尔曼滤波只擅长处理线性系统，怎么办？
泰勒展开就是干这个的——它把一个复杂的非线性函数，在某一点附近展开成多项式。我们通常只取到一阶项，也就是线性项。
核心思想： 在某个点附近，用切线（一阶近似）代替原曲线。
公式长这样：
f(x) ≈ f(a) + f'(a)(x - a)嗯，看起来简单。但我在项目中遇到过一个问题：如果系统变化太快，泰勒展开的近似误差会迅速累积。所以EKF对「运动模型更新频率」和「传感器采样率」是有要求的，不能太慢。
我的经验： 实际调试时，如果EKF发散，先检查泰勒展开的线性化点是否合理。我曾经在一个无人机项目中，因为IMU采样率太低，导致线性化点严重偏离真实状态，滤波器直接炸了。
2. 雅可比矩阵：多维的导数
泰勒展开在一维情况下很简单，就是求导。但在多维系统中，比如一个状态向量有位置、速度、姿态角，每个维度之间相互影响，怎么办？
雅可比矩阵就是多维的导数。它是一个矩阵，每个元素表示一个输出分量对某个输入分量的偏导数。
举个例子，假设状态向量是 [x, y, θ]，运动模型输出是 [x', y', θ']。雅可比矩阵就是：
J = [ ∂x'/∂x ∂x'/∂y ∂x'/∂θ ]
[ ∂y'/∂x ∂y'/∂y ∂y'/∂θ ]
[ ∂θ'/∂x ∂θ'/∂y ∂θ'/∂θ ]说白了，雅可比矩阵告诉我们：状态量的微小变化，会如何影响预测结果。
避坑指南： 我曾经在写EKF代码时，手动推导雅可比矩阵，结果一个偏导数符号写反了。滤波器跑了半小时才发散，排查了一整天。后来我改用符号计算工具（比如SymPy）自动生成雅可比矩阵，再也不用担心手算错误了。
你想想看，如果雅可比矩阵算错了，EKF的预测步和更新步就全歪了。所以我的建议是：
- 先用手算一遍，理解物理意义
- 再用符号工具验证
- 最后在代码里写死
3. 协方差矩阵：不确定性的度量
卡尔曼滤波的核心，就是维护一个「不确定性」的估计。这个不确定性，就是用协方差矩阵来表示的。
协方差矩阵的对角线元素，表示每个状态量的方差（不确定性大小）。非对角线元素，表示两个状态量之间的相关性。
举个例子，如果机器人的位置和速度协方差为负，说明位置偏大时速度偏小——这在实际中可能意味着机器人正在减速。
| 协方差矩阵元素 | 含义 | 实际例子 |
|---|---|---|
| P[0][0] | 位置x的方差 | GPS信号差时，这个值变大 |
| P[1][1] | 速度的方差 | 加速度计噪声大时，这个值变大 |
| P[0][1] | 位置与速度的协方差 | 位置和速度正相关时，值为正 |
关键点： 协方差矩阵必须是半正定的。如果代码里出现负的方差，那一定是bug。我在调试时经常打印协方差矩阵的特征值，如果出现负数，立刻检查更新步的代码。
4. 三者如何协同工作？
好，现在我们把三个工具串起来，看看它们在EKF中是怎么配合的：
- 预测步： 用非线性运动模型预测下一时刻的状态。但模型是非线性的，所以用泰勒展开在上一时刻状态处线性化。
- 线性化工具： 泰勒展开需要导数，而多维导数就是雅可比矩阵。我们用雅可比矩阵来传播协方差矩阵。
- 不确定性传播： 协方差矩阵通过雅可比矩阵进行更新，告诉我们预测的不确定性有多大。
说白了，整个过程就是：
- 用泰勒展开把非线性模型掰直
- 用雅可比矩阵计算掰直后的斜率
- 用协方差矩阵记录不确定性
下面这张图，是我自己画的知识结构图，帮你理清三者关系：
一个小技巧： 刚开始学EKF时，别急着写完整代码。先单独测试雅可比矩阵的计算是否正确——给一个已知的非线性函数，手动算导数，再和代码结果对比。这一步通过了，后面就顺了。
好了，数学基础就讲到这里。这三个工具是EKF的骨架，理解透了，后面的代码实现就是「照着公式填数字」而已。