4. EKF核心原理：线性化与雅可比矩阵

说到EKF，很多初学者第一反应就是「卡尔曼滤波的扩展版」。嗯，这个说法没错，但有点太笼统了。我个人的理解是：EKF就是给标准卡尔曼滤波装上了一副「线性化眼镜」，让它能看清非线性的世界。

你想想看，现实中的机器人系统，有几个是纯线性的？车轮打滑、传感器畸变、运动学模型里的三角函数……到处都是非线性。标准KF面对这些情况，就像让一个近视眼不戴眼镜去开车——能开，但大概率要出事。

4.1 为什么需要线性化？

我记得刚入行时做过一个轮式机器人定位项目。运动模型里有个cos(θ)，观测模型里有个arctan(y/x)。标准KF要求状态转移和观测方程都是线性的，也就是必须写成 x' = A·x + B·u 这种形式。但我的模型里全是三角函数和开方运算，根本套不进去。

当时我硬着头皮把非线性模型当线性用，结果滤波结果发散得一塌糊涂。后来才明白：非线性系统的高斯分布经过非线性变换后，就不再是高斯的了。而卡尔曼滤波的所有推导都建立在「高斯分布经过线性变换后仍是高斯」这个前提上。

说白了，EKF要解决的就是这个问题：在每一个时间步，把非线性函数在当前估计值附近做一阶泰勒展开，得到一个近似的线性模型。这样就能继续用KF的框架了。

4.2 泰勒展开与雅可比矩阵

泰勒展开大家高数都学过，但实际用起来还是有点讲究的。对于一个非线性函数 f(x)，在 x̂ 附近展开：

f(x) ≈ f(x̂) + J·(x - x̂)

这里的 J 就是雅可比矩阵。它本质上是一个一阶偏导数矩阵，描述了函数在每个输入维度上的变化率。

举个例子，假设状态向量是 x = [px, py, θ]ᵀ，运动模型是：

px' = px + v·cos(θ)·dt
py' = py + v·sin(θ)·dt
θ'  = θ + ω·dt

那么状态转移函数的雅可比矩阵 F 就是：

F = ∂f/∂x = 
[1, 0, -v·sin(θ)·dt]
[0, 1,  v·cos(θ)·dt]
[0, 0,  1           ]

4.3 EKF的完整流程

EKF的流程和标准KF几乎一模一样，只是多了两步线性化。我习惯把它分成五个步骤：

1. 预测步（线性化+传播）：用非线性模型预测状态，用雅可比矩阵传播协方差
2. 更新步（线性化+校正）：用非线性观测模型计算残差，用雅可比矩阵计算卡尔曼增益

具体公式如下：

// 预测步
x_pred = f(x_est, u)           // 非线性状态预测
F = ∂f/∂x | x_est              // 状态转移雅可比
P_pred = F·P_est·Fᵀ + Q        // 协方差预测

// 更新步
z_pred = h(x_pred)             // 非线性观测预测
H = ∂h/∂x | x_pred             // 观测雅可比
K = P_pred·Hᵀ·(H·P_pred·Hᵀ + R)⁻¹  // 卡尔曼增益
x_est = x_pred + K·(z - z_pred)     // 状态更新
P_est = (I - K·H)·P_pred            // 协方差更新

你看，和标准KF相比，只是把 A 换成了 F，把 C 换成了 H。但这两个矩阵不再是固定的，而是每个时间步都要重新计算。

4.4 雅可比矩阵的数值计算

有些模型的解析雅可比太难求了，比如复杂的传感器模型或者神经网络。这时候可以用数值方法近似：

// 数值雅可比（前向差分）
for i = 1 to n:
    x_plus = x
    x_plus[i] += epsilon
    J[:, i] = (f(x_plus) - f(x)) / epsilon

但要注意，epsilon 的选择很关键。太大则近似误差大，太小则数值不稳定。我一般取 sqrt(machine_epsilon) * max(1, |x[i]|)。

4.5 EKF的局限性

EKF虽然好用，但也不是万能的。我总结了几点需要注意的地方：

• 线性化误差：一阶泰勒展开只在局部有效。如果系统非线性很强，或者初始误差很大，线性化误差会累积导致发散
• 雅可比计算负担：每个时间步都要计算雅可比矩阵，对于高维系统（比如SLAM）计算量很大
• 高斯假设：EKF仍然假设状态服从高斯分布，对于多模态分布无能为力

我记得有一次做无人机姿态估计，用了EKF但效果一直不好。后来发现是无人机做大机动转弯时，角度变化太快，线性化误差太大。换成UKF（无迹卡尔曼滤波）后就好多了。

4.6 本章知识体系

下面这张图总结了EKF核心原理的知识结构，我画了很久才理清楚：

4.7 什么时候该用EKF？

根据我的经验，EKF最适合以下场景：

• 系统非线性程度不高，或者工作点变化不大
• 状态维度适中（10维以内），雅可比计算可接受
• 对实时性要求较高，需要快速迭代
• 传感器噪声近似高斯分布

如果系统非线性很强，或者状态维度很高，我建议考虑UKF或粒子滤波。但话说回来，EKF仍然是工程中最常用的非线性滤波方法，没有之一。它的实现简单、计算高效，对于大多数实际应用已经足够。

---