【扩展卡尔曼滤波公式推导与工程实现】【理论】【2/3 雅可比矩阵与线性化】

1. 从线性到非线性：为什么标准卡尔曼滤波不够用了？

如果你已经跟着我之前的文章，把标准卡尔曼滤波的推导和代码都跑通了，那你肯定已经感受到了它的强大。它就像一个超级精准的“预测-修正”机器，只要你的系统是线性的，噪声是高斯白噪声，它就能给出理论上最优的状态估计。我在做无人机飞控和机器人定位项目时，用它来融合IMU和GPS数据，效果稳得一批。

但现实世界哪有那么多“线性”的好事？我踩的第一个大坑，就是给一个四旋翼无人机做姿态估计。它的运动方程里，姿态角（滚转、俯仰、偏航）的更新天生就是非线性的，涉及到三角函数。你没法用一个固定的矩阵 A 去描述它从上一秒姿态到下一秒姿态的变化。还有，用摄像头追踪一个做圆周运动的物体，它的状态转移方程也是非线性的。这时候，你把标准卡尔曼滤波那套公式硬套上去，结果往往会发散，估计值跑得比真值还快，完全没法用。

这就是标准卡尔曼滤波的核心局限：它要求状态转移和观测模型都必须是线性的。也就是说，下一时刻的状态 z_k 必须是上一时刻状态 z_{k-1} 的线性函数（z_k = A * z_{k-1} + ...），观测值 y_k 也必须是当前状态 z_k 的线性函数（y_k = H * z_k + ...）。只有在这个前提下，高斯分布经过线性变换后还是高斯分布，我们那套基于协方差矩阵递推的最优估计理论才成立。

一旦模型变成 z_k = f(z_{k-1}) 和 y_k = h(z_k)，其中 f 和 h 是非线性函数，所有美好的性质就都消失了。非线性函数会扭曲状态的概率分布，它可能不再是高斯的，期望和协方差的传播也变得极其复杂，无法用简单的矩阵乘法来递推。

那怎么办呢？难道面对非线性系统我们就束手无策了吗？当然不是。工程师们的智慧是无穷的，我们的策略是：局部线性化。既然全局上不是线性的，那我在每一个时刻，系统当前估计值的附近，把它近似成一个线性系统不就行了？这就好比你在一条弯曲的山路上开车，虽然整条路是弯的，但在任何一个瞬间，你都可以把方向盘打直，沿着当前点的切线方向开一小段。扩展卡尔曼滤波（EKF）干的就是这个事，而实现这个“局部切线近似”的数学工具，就是一阶泰勒展开和它的执行者——雅可比矩阵。

所以，EKF的核心思想可以概括为：在每一个滤波周期，我们都以当前的最优估计值为中心，对非线性的 f 和 h 函数进行一阶泰勒展开，得到它们在该点的线性近似。然后用这个近似出来的线性系统，去套用标准卡尔曼滤波的公式。这样一来，我们每一步都是在处理一个“局部线性”的问题，通过不断地重新线性化，来逼近整个非线性过程。

2. 雅可比矩阵：打开非线性系统线性化大门的钥匙

好了，现在我们知道了要用一阶泰勒展开来线性化。但具体怎么操作呢？对于一个多输入、多输出的向量函数，一阶泰勒展开长什么样？这就轮到我们今天的主角——雅可比矩阵（Jacobian Matrix）闪亮登场了。

2.1 重温一阶泰勒展开：从单变量到多变量

我们先从最熟悉的地方开始。对于一个单变量非线性函数 y = f(x)，在点 x0 处的一阶泰勒展开是： f(x) ≈ f(x0) + f'(x0) * (x - x0) 这里 f'(x0) 就是函数在 x0 处的导数，它代表了函数在该点的局部斜率，也就是线性近似的系数。

对于多变量函数，比如 z = f(x, y)，在点 (x0, y0) 处的一阶泰勒展开是： f(x, y) ≈ f(x0, y0) + ∂f/∂x |_(x0,y0) * (x - x0) + ∂f/∂y |_(x0,y0) * (y - y0) 这里，∂f/∂x 和 ∂f/∂y 是两个偏导数，它们共同构成了一个梯度向量，指向函数增长最快的方向。

那么，对于我们的状态估计问题，状态 z 本身就是一个向量（比如 [x, y, vx, vy]^T），函数 f 和 h 也是输出向量的函数。这时候，我们需要把上面这个思想推广到向量对向量的求导。

2.2 雅可比矩阵的定义与计算

假设我们有一个函数，它输入一个 n 维向量 x，输出一个 m 维向量 y： y = f(x) = [f1(x), f2(x), ..., fm(x)]^T x = [x1, x2, ..., xn]^T

这个函数 f 在点 x0 处的雅可比矩阵 J，就是一个 m x n 的矩阵，它的第 i 行第 j 列元素，就是第 i 个输出函数 fi 对第 j 个输入变量 xj 的偏导数，在 x0 处求值：

J = ∂f/∂x |_{x=x0} = 
[ ∂f1/∂x1, ∂f1/∂x2, ..., ∂f1/∂xn ]
[ ∂f2/∂x1, ∂f2/∂x2, ..., ∂f2/∂xn ]
[ ...                       ... ]
[ ∂fm/∂x1, ∂fm/∂x2, ..., ∂fm/∂xn ] (在 x0 处取值)

你可以把雅可比矩阵理解为“向量函数的梯度矩阵”。对于单输出函数，梯度是一个向量，指向上升最快的方向。对于多输出函数，雅可比矩阵的每一行就是对应输出分量的梯度向量。它完整地描述了在 x0 这个点附近，输入向量 x 的每一个微小变化，会如何影响输出向量 y 的每一个分量。

有了雅可比矩阵，向量函数的一阶泰勒展开就可以漂亮地写成矩阵形式： f(x) ≈ f(x0) + J * (x - x0)

看，这和单变量的形式 f(x) ≈ f(x0) + f'(x0)*(x - x0) 在结构上是一模一样的！只是导数 f'(x0) 换成了雅可比矩阵 J<