上篇把卡尔曼滤波的预测-更新循环讲透了——蒙眼走路的直觉、五个核心方程、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在当前估计点做线性化如果初始估计偏差太大线性化就不准了。这就好比你在一座山的半山腰用平面去近似山坡——如果你在山脚就开始近似近似出来的平面可能完全不对。解决办法是保证初始估计不要偏太远或者用迭代EKFIEKF——在更新步中多次线性化逐步逼近真实值。第二个坑雅可比矩阵算错了。前面说了手推雅可比容易出错。工程中有两种应对方式一是用自动微分工具比如C的CppAD、Python的JAX让计算机帮你算偏导数二是干脆用UKF完全跳过雅可比矩阵。之前做AMR项目的时候我们团队有人在雅可比矩阵里把一个sin写成了cosdebug了三天才找到。第三个坑角度归一化。机器人状态里有航向角yaw这个值在-pi到pi之间。做状态更新的时候x_est x_pred K innovationinnovation里的角度差可能跨越±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——不用算雅可比的替代方案有任何问题欢迎评论区留言我会尽量回复。