上篇把卡尔曼滤波的预测-更新循环讲透了——蒙眼走路的直觉、五个核心方程、Q和R的工程调参。但卡尔曼滤波有个硬伤：它要求系统是线性的。

真实世界的机器人系统，几乎都不是线性的。

你想想，机器人转弯的时候，航向角和位置之间的关系是三角函数；视觉SLAM中，三维空间点投影到二维图像平面，这个投影变换也是非线性的。怎么办？扩展卡尔曼滤波（EKF）就是来解决这个问题的。

面试中，EKF的出现频率比标准卡尔曼滤波还高。因为实际工程中你用的基本都是EKF或者它的变种。今天这篇，把EKF的核心思路、雅可比矩阵的作用、以及工程中的坑，一次讲清楚。

非线性在哪里？

先搞清楚"非线性"到底指什么。标准卡尔曼滤波有两个方程：

状态转移：x_k = F * x_{k-1} + B * u_k + w_k 观测方程：z_k = H * x_k + v_k

这两个方程都是线性的——状态乘个矩阵就得到下一步状态，状态乘个矩阵就得到测量值。

但实际系统中，状态转移可能是这样的：

# 机器人运动学模型（非线性）
x_new = x + v * cos(yaw) * dt
y_new = y + v * sin(yaw) * dt
yaw_new = yaw + omega * dt

这里cos和sin让状态转移变成了非线性函数。观测方程也可能非线性——比如你用激光雷达测到某个路标的距离和角度，从路标的位置反推测量值，需要开根号和arctan。

EKF的思路特别直接：既然卡尔曼滤波只能处理线性，那我就在当前估计点附近把非线性函数线性化。怎么线性化？泰勒展开，取一阶项。

雅可比矩阵：线性化的工具

泰勒展开到一阶，核心就是求雅可比矩阵。雅可比矩阵说白了就是"非线性函数在某一点对各变量的偏导数组成的矩阵"。

对于状态转移函数f(x)，雅可比矩阵F_jac就是f对x的偏导：

# 雅可比矩阵（状态转移的线性化）
F_jac[i][j] = ∂f[i] / ∂x[j]

对于观测函数h(x)，雅可比矩阵H_jac就是h对x的偏导：

# 雅可比矩阵（观测的线性化）
H_jac[i][j] = ∂h[i] / ∂x[j]

EKF的五个方程，和标准卡尔曼滤波几乎一模一样，只是把F换成了F_jac，把H换成了H_jac：

# EKF预测步
x_pred = f(x_prev, u)  # 用非线性函数预测
P_pred = F_jac @ P_prev @ F_jac.T + Q

# EKF更新步
K = P_pred @ H_jac.T @ inv(H_jac @ P_pred @ H_jac.T + R)
x_est = x_pred + K @ (z - h(x_est_pred))
P_est = (I - K @ H_jac) @ P_pred

注意区别：状态预测用的是原始非线性函数f，协方差预测用的是雅可比矩阵F_jac。这两者不一样——f负责把状态"推"到下一步，F_jac负责把不确定性"推"到下一步。

一个具体例子：二维机器人定位

假设一个差速驱动机器人在二维平面运动，状态是[x, y, yaw]，控制量是线速度v和角速度omega。运动学模型是：

def motion_model(x, y, yaw, v, omega, dt):
    if abs(omega) < 1e-6:  # 角速度接近零
        x_new = x + v * cos(yaw) * dt
        y_new = y + v * sin(yaw) * dt
    else:
        x_new = x + v/omega * (sin(yaw + omega*dt) - sin(yaw))
        y_new = y + v/omega * (cos(yaw) - cos(yaw + omega*dt))
    yaw_new = yaw + omega * dt
    return x_new, y_new, yaw_new

这个模型里cos和sin就是非线性的来源。对应的雅可比矩阵需要手动推导：

# 运动学雅可比矩阵（对x, y, yaw求偏导）
F_jac = np.array([
    [1, 0, -v*sin(yaw)*dt],
    [0, 1,  v*cos(yaw)*dt],
    [0, 0,  1]
])

讲真，推导雅可比矩阵是EKF中最容易出bug的地方。状态向量一多、运动模型一复杂，手推雅可比矩阵非常容易算错。这也是为什么后来有了无迹卡尔曼滤波（UKF）——不用算雅可比，下篇会讲。

工程中的三大坑

EKF在工程应用中，有几个教科书不会告诉你的坑。

第一个坑：线性化点选错了。EKF在当前估计点做线性化，如果初始估计偏差太大，线性化就不准了。这就好比你在一座山的半山腰用平面去近似山坡——如果你在山脚就开始近似，近似出来的平面可能完全不对。解决办法是：保证初始估计不要偏太远，或者用迭代EKF（IEKF）——在更新步中多次线性化，逐步逼近真实值。

第二个坑：雅可比矩阵算错了。前面说了，手推雅可比容易出错。工程中有两种应对方式：一是用自动微分工具（比如C++的CppAD、Python的JAX），让计算机帮你算偏导数；二是干脆用UKF，完全跳过雅可比矩阵。之前做AMR项目的时候，我们团队有人在雅可比矩阵里把一个sin写成了cos，debug了三天才找到。

第三个坑：角度归一化。机器人状态里有航向角yaw，这个值在-pi到pi之间。做状态更新的时候，x_est = x_pred + K @ innovation，innovation里的角度差可能跨越±pi边界。比如预测航向是3.1rad，测量航向是-3.1rad，实际角度差只有0.18rad，但直接减会得到-6.2rad。必须在计算新息之前做角度归一化（用atan2(sin(diff), cos(diff))）。这种bug在仿真中可能看不出来，上了真车就会莫名其妙地发散。

面试中怎么聊

面试官问EKF，按这个思路回答：先说标准卡尔曼滤波只能处理线性系统，实际机器人系统几乎都是非线性的，所以需要EKF。然后说EKF的核心思路——在当前估计点用泰勒展开做一阶线性化，线性化的工具就是雅可比矩阵。再说EKF和标准KF的区别——只是把F和H换成了雅可比矩阵，其他框架不变。最后说工程中的坑（线性化点偏差、雅可比计算、角度归一化）。

如果面试官追问"EKF的缺点是什么"，你可以说：EKF的一阶线性化在强非线性系统中精度不够。比如机器人急转弯的时候，运动模型中的三角函数变化剧烈，一阶近似误差很大。这时候有两种选择：迭代EKF（多次线性化提高精度）或者UKF（不用线性化，用sigma点采样）。另外，EKF假设后验分布是高斯的，在多模态分布的场景下（比如全局定位——机器人可能在走廊的任何位置），EKF也不适用，这时候要用粒子滤波。

如果面试官追问"雅可比矩阵怎么验证对不对"，你可以说：最简单的方法是数值验证——用有限差分法算出来的数值雅可比和你手推的解析雅可比做对比。如果两者在误差范围内一致，说明解析雅可比推导正确。具体做法是：对每个状态分量加一个小扰动delta，算出函数值的变化量，除以delta得到数值偏导数。这个方法虽然计算量大（不适合实时运行），但用来离线验证雅可比矩阵的正确性非常好用。

分享一个我在EKF调试中遇到的典型问题。当时做移动机器人的EKF定位融合，状态量是[x, y, yaw, vx, vy, omega]六维。一开始滤波器在直线行驶时表现正常，但一转弯就发散。排查了很久最后发现是角度归一化的问题——在计算新息（innovation）时，yaw角的差值没有做归一化处理。比如机器人转弯时yaw从3.1变到-3.1，实际变化只有0.08弧度，但直接相减得到-6.2弧度，滤波器以为状态偏差巨大，直接发了一个超大的修正量，导致发散。加上atan2(sin(diff), cos(diff))做角度归一化后，转弯时的发散问题立刻消失了。这个bug在直线行驶时完全看不出来，只有转弯时才会触发。面试时候提到这种角度归一化的坑，面试官会知道你真的写过EKF的代码。

下一篇讲无迹卡尔曼滤波UKF——一种不需要计算雅可比矩阵的替代方案。

---

如果这篇文章对你有帮助，欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。

「机器人软件开发面试·从入门到精通」连载系列 上一篇：第166篇 卡尔曼滤波详解——预测-更新循环的直觉理解 下一篇预告：第168篇 无迹卡尔曼滤波UKF——不用算雅可比的替代方案

有任何问题欢迎评论区留言，我会尽量回复。