扩展卡尔曼滤波EKF——非线性系统的状态估计

发布时间:2026/8/16 21:45:16

扩展卡尔曼滤波EKF——非线性系统的状态估计
上篇把卡尔曼滤波的预测-更新循环讲透了——蒙眼走路的直觉、五个核心方程、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——不用算雅可比的替代方案有任何问题欢迎评论区留言我会尽量回复。

相关新闻

群晖Docker部署Odoo 13:低成本构建企业级ERP系统

群晖Docker部署Odoo 13:低成本构建企业级ERP系统

2026/8/16 21:45:16

1. 项目概述:为什么要在群晖Docker里部署Odoo 13?如果你手头有一台群晖NAS,并且正在为小团队或初创公司寻找一个成本可控、功能全面又易于管理的企业资源计划(ERP)系统,那么“在群晖Docker里跑Odoo 13”这个…

Table as a Modality for Large Language Models

Table as a Modality for Large Language Models

2026/8/16 21:35:15

论文核心内容与创新点总结 一、主要内容 本文聚焦大语言模型(LLMs)在表格推理任务中的局限性,提出将表格视为独立模态的解决方案。现有LLMs处理表格时多采用序列化文本转换方式,导致表格特有的结构信息(如排列不变性)丢失,进而影响推理性能。为此,作者设计了StructQA…

从PE启动盘制作到UEFI引导修复:手把手解决Windows系统无法启动问题

从PE启动盘制作到UEFI引导修复:手把手解决Windows系统无法启动问题

2026/8/16 21:35:15

1. 项目缘起:一次硬盘升级引发的“系统消失”事件 上个月,我帮朋友升级他的老电脑,把用了五年的机械硬盘换成了固态硬盘。过程很顺利,系统迁移、数据拷贝一气呵成。但就在拔掉旧硬盘,准备用新盘独立启动时,…

手把手教你学 Simulink—— 群体无人机协同覆盖路径生成

手把手教你学 Simulink—— 群体无人机协同覆盖路径生成

2026/8/16 22:45:19

目录 一、为什么群体覆盖要进 Simulink?难点在哪? 1.1 纯算法视角(Python/C++ 脚本) 1.2 纯控制视角(Simulink 原生) 1.3 覆盖规划 + Simulink 互补(核心思想) 二、仿真总体架构 三、关键参数(教学默认) 四、Simulink 建模 Step‑by‑Step Step ① —— 构建…

LangChain与LangGraph深度解析:从入门到生产级Agent开发

LangChain与LangGraph深度解析:从入门到生产级Agent开发

2026/8/16 22:45:19

前言2026年,AI Agent已经成为大模型落地的核心范式。如果你还在裸调大模型API,可能已经落后了。LangChain和LangGraph是目前构建现代AI应用最广泛使用的两个框架,两者结合提供了构建可靠、可扩展、易维护AI系统所需的核心构件。本文带你一文搞…

阅读账本用户协议(v1.0,生效日期:2026-08-14)

阅读账本用户协议(v1.0,生效日期:2026-08-14)

2026/8/16 22:45:18

# 阅读账本用户协议**协议名称**:《阅读账本用户协议》**版本**:v1.0**生效日期**:2026-08-14**开发者**:解于洋**联系方式**:xieyy1998163.com> 欢迎使用「阅读账本」。在使用本应用前,请仔细阅读并充分…

手把手教你学 Simulink—— 半导体光刻机工件台永磁直线电机的无模型自适应控制仿真

手把手教你学 Simulink—— 半导体光刻机工件台永磁直线电机的无模型自适应控制仿真

2026/8/16 22:45:18

目录 手把手教你学 Simulink —— 半导体光刻机工件台永磁直线电机的无模型自适应控制仿真 一、为什么光刻机工件台"不能靠精确建模"?

智能体记忆错误修复:依赖引导回滚机制的设计与实现

智能体记忆错误修复:依赖引导回滚机制的设计与实现

2026/8/16 22:45:18

1. 项目概述:当智能体“记错”时,如何让它“回滚”并自我修复? 在构建具备长期记忆能力的智能体(Memory-Augmented Agents)时,我们总会遇到一个令人头疼的问题:智能体基于记忆做出了错误的决策或…

Linux性能优化工具系列详解(1)

Linux性能优化工具系列详解(1)

2026/8/16 22:35:18

本系列内容参考:极客时间 —— 倪朋飞 《Linux性能优化实战》特此致谢!序言Linux系统性能优化,是基于Linux操作系统软硬件全栈,通过监控定位瓶颈、调整内核参数、优化调度/存储/网络、调整应用架构、合理分配硬件资源等手段&#…

【文章复现】非线性值迭代自适应动态规划(ADP):离散时间非线性系统的策略迭代自适应动态规划算法研究附Matlab代码

【文章复现】非线性值迭代自适应动态规划(ADP):离散时间非线性系统的策略迭代自适应动态规划算法研究附Matlab代码

2026/8/16 0:04:13

✅作者简介:热爱科研的Matlab仿真开发者,擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。🍎 往期回顾关注个人主页:Matlab科研工作室👇 关注我领取海量matlab电子书和…

【双层规划,节点出清价,绿证交易,CVaR方法】两级电力市场环境下计及风险的省间交易商最优购电模型附Matlab代码

【双层规划,节点出清价,绿证交易,CVaR方法】两级电力市场环境下计及风险的省间交易商最优购电模型附Matlab代码

2026/8/16 0:04:13

✅作者简介:热爱科研的Matlab仿真开发者,擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。🍎 往期回顾关注个人主页:Matlab科研工作室👇 关注我领取海量matlab电子书和…

隐式mpc+自适应mpc+时变mpc,线性时变模型预测控制附Simulink仿真

隐式mpc+自适应mpc+时变mpc,线性时变模型预测控制附Simulink仿真

2026/8/16 0:04:13

✅作者简介:热爱科研的Matlab仿真开发者,擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。🍎 往期回顾关注个人主页:Matlab科研工作室👇 关注我领取海量matlab电子书和…

【文章复现】非线性值迭代自适应动态规划(ADP):离散时间非线性系统的策略迭代自适应动态规划算法研究附Matlab代码

【文章复现】非线性值迭代自适应动态规划(ADP):离散时间非线性系统的策略迭代自适应动态规划算法研究附Matlab代码

2026/8/16 0:04:13

✅作者简介:热爱科研的Matlab仿真开发者,擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。🍎 往期回顾关注个人主页:Matlab科研工作室👇 关注我领取海量matlab电子书和…

【双层规划,节点出清价,绿证交易,CVaR方法】两级电力市场环境下计及风险的省间交易商最优购电模型附Matlab代码

【双层规划,节点出清价,绿证交易,CVaR方法】两级电力市场环境下计及风险的省间交易商最优购电模型附Matlab代码

2026/8/16 0:04:13

✅作者简介:热爱科研的Matlab仿真开发者,擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。🍎 往期回顾关注个人主页:Matlab科研工作室👇 关注我领取海量matlab电子书和…

隐式mpc+自适应mpc+时变mpc,线性时变模型预测控制附Simulink仿真

隐式mpc+自适应mpc+时变mpc,线性时变模型预测控制附Simulink仿真

2026/8/16 0:04:13

✅作者简介:热爱科研的Matlab仿真开发者,擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。🍎 往期回顾关注个人主页:Matlab科研工作室👇 关注我领取海量matlab电子书和…

摆脱论文困扰!盘点2026年全网爆红的的AI论文写作工具

摆脱论文困扰!盘点2026年全网爆红的的AI论文写作工具

2026/8/15 1:04:46

一天写完毕业论文在2026年已不再是天方夜谭。2026年最炸裂、实测能大幅提速的AI论文写作工具,覆盖选题构思、文献整理、内容生成、格式排版等核心场景,真正帮你高效搞定论文难题。 一、全流程王者:一站式搞定论文全链路(一天定稿首…

导师推荐!2026最新AI论文工具测评与实用推荐

导师推荐!2026最新AI论文工具测评与实用推荐

2026/8/15 10:10:27

2026年真正好用的AI论文工具,核心看生成的论文质量、低AI味、格式正确、学术适配四大指标。综合实测,千笔AI、ThouPen、豆包、DeepSeek、Grammarly 是当前最值得推荐的梯队,覆盖从免费到付费、从中文到英文、从文科到理工的全场景需求。 一、…

告别游戏崩溃:XCOM 2模组管理器的智能革命

告别游戏崩溃:XCOM 2模组管理器的智能革命

2026/8/14 19:35:14

告别游戏崩溃:XCOM 2模组管理器的智能革命 【免费下载链接】xcom2-launcher The Alternative Mod Launcher (AML) is a replacement for the default game launchers from XCOM 2 and XCOM Chimera Squad. 项目地址: https://gitcode.com/gh_mirrors/xc/xcom2-lau…