简介这份资源围绕扩展卡尔曼滤波EKF展开面向从事惯性导航、姿态估计与多传感器融合的开发者及研究生重点解决9维IMU在姿态跟踪中的滤波实现问题。内容参考《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》论文给出基于EKF-IMU与ESKF-IMU两套算法的实现代码便于读者对照论文理解状态方程、观测模型与误差状态更新的差异。压缩包为zip格式整体约2.23MB文件类型以源码与工程文件为主可直接用于算法验证与二次开发。目前已有441人学习下载适合希望从理论推导过渡到工程落地、需要现成参考实现来调试姿态解算效果的读者也可作为课程设计或科研项目的起步素材。1. 扩展卡尔曼滤波一个让机器人不跑偏的“导航大脑”你有没有想过扫地机器人在你家客厅里转来转去怎么就知道自己现在在哪儿、下一步该往哪走它没有眼睛轮子还会打滑可它就是能一圈圈把地扫干净。这背后有个东西在默默干活叫扩展卡尔曼滤波英文缩写 EKF。说白了它就是一个“猜位置”的数学工具专门对付那些“不太听话”的系统——比如轮子会打滑、传感器有噪声、运动规律不是一条直线。普通卡尔曼滤波只能处理线性系统就像只能走直路而现实世界大多是弯的EKF 就是那个允许你走弯路的升级版。搞懂它你就明白为什么无人机能悬停、手机能定位、自动驾驶能跟车——这些场景里EKF 几乎无处不在。2. 为什么需要 EKF从“只能走直线”到“允许拐弯”2.1 卡尔曼滤波的局限线性假设太苛刻卡尔曼滤波诞生在上世纪六十年代一开始是为了解决阿波罗登月里的导航问题。它的核心思想很朴素你有一个“预测”还有一个“测量”两者都不准那就按各自的不确定度加权平均一下得到一个更准的估计。这个思路在线性系统里是最优的数学上漂亮得没话说。但问题来了什么叫线性系统打个比方你开车油门踩多少速度就涨多少方向打多少车头就转多少而且这个关系是成比例的、可叠加的。可现实里你踩油门速度不是线性涨的风阻是平方关系你打方向车走的轨迹是圆弧角度一变位置变化就不是简单乘法了。这些“弯的”关系卡尔曼滤波处理不了硬套上去就会翻车。注意很多初学者以为卡尔曼滤波就是一个公式背下来就行。其实它是一套“预测-更新”的递推框架线性假设是它的命门。2.2 EKF 的解法用“切线”把弯的掰直EKF 的做法很聪明既然整个曲线我搞不定那我就在当前估计点附近用一条切线去近似它。这条切线就是“线性化”。数学上叫一阶泰勒展开你不用记这个名字只需要知道EKF 在每个时刻都把非线性的运动模型和观测模型在当前估计值附近“掰直”成线性的然后再套用卡尔曼滤波的那套加权平均。这就像你要测量一个弯曲山坡的坡度你不可能一次测完整座山但你可以站在当前位置用一块小直板贴住地面量出这一点的坡度。EKF 就是不断在移动中做这件事每一步都重新“贴板子”所以它能跟着系统走。代价是什么线性化会引入误差。如果系统弯得太厉害或者你估计的点离真实值太远切线就偏得离谱EKF 可能发散——也就是越估越错最后彻底跑飞。这也是为什么后来有人搞出了无迹卡尔曼滤波UKF和粒子滤波它们不用线性化但计算量更大。EKF 的定位就是在精度和计算量之间找一个平衡点够用、快、好实现。2.3 一个生活化类比蒙眼找路想象你被蒙上眼睛站在一个广场上手里有个 GPS 告诉你大概位置但误差有十米。你每走一步心里默数步数和方向估算自己移动了多少。GPS 读数来了你就跟自己的估算对比一下如果 GPS 说你在东边你心里估算也在东边那就信自己多一点如果 GPS 突然跳到西边你心里估算没动那就怀疑 GPS 抽风了少信它一点。EKF 干的就是这件事只不过它用数学严格定义了“信谁多一点”——用协方差矩阵表示不确定度。你走得越久自己估算的不确定度越大就更信 GPSGPS 信号差的时候不确定度大就更信自己的推算。整个过程是递归的不需要存历史数据每一步只依赖上一步的估计和当前测量所以特别适合跑在单片机上。3. EKF 的核心零件预测、更新与雅可比矩阵3.1 预测步用运动模型往前推EKF 每个循环分两步第一步叫预测。你有一个状态向量比如机器人的位置 x、y 和朝向角 θ还有速度 v 和角速度 ω。根据上一时刻的状态你用运动学公式推算这一时刻的状态。如果是非线性函数 f就直接代进去算。同时你还要预测不确定度。不确定度用一个协方差矩阵 P 表示它描述了你对每个状态量的信心。预测时P 会变大因为推得越远越不准。这里的关键是非线性函数 f 不能直接乘 P需要先求它的雅可比矩阵 F。雅可比矩阵就是 f 对每个状态量的偏导数组成的矩阵它告诉你在当前点状态微小变化会如何影响下一步。说白了它就是“局部斜率”的多维版本。import numpy as np def predict(x, P, u, dt): # x: 状态向量 [x, y, theta, v, omega] # u: 控制输入这里假设为0实际可含加速度 # dt: 时间步长 theta x[2] v x[3] omega x[4] # 非线性运动模型 if abs(omega) 1e-6: x_new x[0] (v/omega) * (np.sin(theta omega*dt) - np.sin(theta)) y_new x[1] (v/omega) * (-np.cos(theta omega*dt) np.cos(theta)) else: x_new x[0] v * np.cos(theta) * dt y_new x[1] v * np.sin(theta) * dt theta_new theta omega * dt x_pred np.array([x_new, y_new, theta_new, v, omega]) # 雅可比矩阵 F对状态量求偏导 F np.eye(5) if abs(omega) 1e-6: F[0,2] (v/omega) * (np.cos(theta omega*dt) - np.cos(theta)) F[0,3] (np.sin(theta omega*dt) - np.sin(theta)) / omega F[0,4] (v/omega**2) * (np.sin(theta) - np.sin(theta omega*dt)) (v*dt/omega) * np.cos(theta omega*dt) F[1,2] (v/omega) * (np.sin(theta omega*dt) - np.sin(theta)) F[1,3] (-np.cos(theta omega*dt) np.cos(theta)) / omega F[1,4] (v/omega**2) * (np.cos(theta omega*dt) - np.cos(theta)) (v*dt/omega) * np.sin(theta omega*dt) else: F[0,2] -v * np.sin(theta) * dt F[0,3] np.cos(theta) * dt F[1,2] v * np.cos(theta) * dt F[1,3] np.sin(theta) * dt F[2,4] dt # 过程噪声协方差 Q需要根据实际调 Q np.diag([0.1, 0.1, 0.01, 0.1, 0.1]) P_pred F P F.T Q return x_pred, P_pred上面这段代码里F就是雅可比矩阵Q是过程噪声表示你对运动模型有多不信任。Q调大滤波器更信测量调小更信预测。这个参数没有标准答案得根据轮子打滑程度、地面摩擦来试。3.2 更新步用测量修正预测预测完你有了一个先验估计。接下来传感器来数据了比如激光雷达测到机器人离墙 1.2 米或者 GPS 给出经纬度。你把预测的状态代入观测方程 h算出“如果我在预测的位置传感器应该看到什么”。然后拿这个预测的观测值和实际观测值相减得到残差。残差乘以卡尔曼增益 K就得到修正量。K 的大小取决于预测不确定度 P_pred 和测量噪声 R 的比值。P_pred 大、R 小K 就大更信测量反之更信预测。更新完P 会变小因为融合了新的信息信心增加了。观测方程 h 通常也是非线性的比如从位置算距离和角度涉及平方和开方、反正切。所以更新步同样需要 h 的雅可比矩阵 H。H 的每一行对应一个观测量每一列对应一个状态量元素是偏导数。def update(x_pred, P_pred, z, R): # z: 实际观测 [range, bearing] # R: 测量噪声协方差 def h(x): px, py x[0], x[1] r np.sqrt(px**2 py**2) b np.arctan2(py, px) return np.array([r, b]) # 雅可比矩阵 H px, py x_pred[0], x_pred[1] r np.sqrt(px**2 py**2) H np.zeros((2, 5)) H[0,0] px / r H[0,1] py / r H[1,0] -py / (r**2) H[1,1] px / (r**2) z_pred h(x_pred) y z - z_pred # 角度残差归一化到 [-pi, pi] y[1] (y[1] np.pi) % (2*np.pi) - np.pi S H P_pred H.T R K P_pred H.T np.linalg.inv(S) x_new x_pred K y P_new (np.eye(5) - K H) P_pred return x_new, P_new这段代码里H是观测雅可比R是测量噪声。角度残差归一化那一步很容易被忽略但如果不做角度从 179 度跳到 -179 度时残差会变成 358 度滤波器直接懵掉。这是血泪经验新手必踩。3.3 雅可比矩阵EKF 的“方向盘”雅可比矩阵在 EKF 里扮演的角色就像方向盘在车里的角色。它不直接决定你去哪但它决定了你对“当前该往哪修正”的判断。如果雅可比算错了滤波器就会往错误方向修正轻则收敛慢重则发散。很多人第一次写 EKF运动模型和观测模型都写对了但结果就是不对最后发现是雅可比求导漏了一项或者符号搞反了。建议的做法是先用数值差分验证雅可比。比如对每个状态量加一个小扰动 ε算函数值变化再除以 ε跟解析雅可比对比。如果误差在 1e-6 量级说明推导正确。提示数值差分验证雅可比是调试 EKF 最有效的手段没有之一。花十分钟写个验证脚本能省你十小时抓头发。4. 易混概念EKF、KF、UKF、粒子滤波到底怎么分4.1 卡尔曼滤波KF与 EKF线性与非线性KF 和 EKF 最核心的区别就一个系统是不是线性的。KF 要求运动模型和观测模型都是矩阵乘法形式x_{k1} A x_k B u_kz_k C x_k。EKF 允许 f 和 h 是任意非线性函数代价是每步要算雅可比而且只是局部近似。什么时候用 KF如果你的系统本身就是线性的比如匀速直线运动、电路里的电压电流关系那就用 KF计算量小而且是最优估计。什么时候用 EKF只要模型里有三角函数、平方、开方、指数基本就得 EKF。但注意EKF 不是全局最优只是局部近似初值给得太离谱可能不收敛。4.2 EKF 与 UKF线性化 vs 采样UKF 全称无迹卡尔曼滤波它的思路跟 EKF 完全不同。EKF 是把非线性函数线性化UKF 是选一组确定性的采样点叫 sigma 点把这些点代入非线性函数算变换后的均值和协方差。它不需要求雅可比而且对强非线性系统往往比 EKF 更准。那为什么还用 EKF因为 UKF 计算量更大尤其是状态维度高的时候sigma 点数量是 2n1n 是状态维数。如果 n20就要算 41 个点每个点都要过一遍非线性函数。EKF 只需要算一个雅可比矩阵虽然求导麻烦但算起来快。所以嵌入式系统里 EKF 仍然很常见UKF 多用在精度要求高、算力也够的场景。4.3 EKF 与粒子滤波高斯假设的边界粒子滤波完全不假设分布是高斯它用一堆随机样本粒子来表示概率分布能处理多峰、非高斯的情况。比如机器人被绑架到另一个房间全局定位问题粒子滤波就能处理EKF 直接歇菜因为它只有一个高斯峰没法同时表示“我可能在 A 房间也可能在 B 房间”。但粒子滤波计算量巨大粒子少了精度不够粒子多了跑不动。而且它有随机性每次跑结果可能略有不同。EKF 是确定性的给定初值和参数每次结果一样便于调试和复现。所以选型逻辑是如果系统基本是单峰、近似高斯EKF 够用且高效如果多峰、强非线性再考虑粒子滤波。4.4 常见误区EKF 不是“更准的 KF”很多人以为 EKF 是 KF 的升级版精度一定更高。这是个误解。EKF 的精度取决于线性化误差。如果系统非线性很强或者工作点离真实值很远EKF 可能比 KF 还差甚至发散。KF 在线性系统里是最优的EKF 在非线性系统里只是“近似最优”而且这个近似好不好得看具体问题。另一个误区是“Q 和 R 随便设”。Q 是过程噪声R 是测量噪声它们直接决定卡尔曼增益影响滤波器的“性格”。Q 设太小滤波器太自信测量来了也不怎么修正响应慢Q 设太大滤波器太敏感测量噪声全放进来输出抖动。调 Q 和 R 是 EKF 实战里最耗时间的环节没有之一。5. 什么时候用 EKF什么时候别用一个具体例子假设你在做一个模拟项目让一个小车在走廊里沿着墙走车上有一个轮速计和一个激光雷达。轮速计给出速度但轮子会打滑激光雷达测距但偶尔有反射噪声。你想估计小车的位置和朝向。这个场景就是 EKF 的典型用武之地。状态量是 x、y、θ运动模型是非线性的因为 θ 在三角函数里观测模型也是非线性的距离和角度。EKF 能跑在单片机上实时性没问题。你需要做的就是写运动模型、写观测模型、求雅可比、调 Q 和 R。但如果你把场景换成小车在整栋楼里找自己可能在任何楼层任何房间初始位置完全未知。这时候 EKF 就不合适了因为它的高斯假设只能表示一个峰没法同时覆盖多个可能位置。你应该用粒子滤波或者多假设跟踪。再换一个场景小车在一条笔直的铁轨上跑轮速计测速度GPS 测位置。这就是线性系统直接用 KF没必要上 EKF。上 EKF 反而多算雅可比浪费算力还可能因为线性化引入不必要的误差。所以判断标准很清晰第一系统是不是非线性的不是就用 KF。第二分布是不是近似单峰高斯不是就考虑粒子滤波。第三算力够不够够且精度要求高可以试 UKF不够就 EKF。这三个问题问完选型基本就定了。我自己的习惯是每次拿到一个新系统先花半小时把运动模型和观测模型写成代码用数值差分验证雅可比然后跑一段离线数据看残差是不是零均值白噪声。如果残差有偏或者自相关说明模型有问题或者 Q、R 没调好。这个流程走一遍能避开大部分翻车现场。从那以后我每次上 EKF 之前都强制走一遍这个检查再也没出现过滤波器发散到外太空的情况。希望帮到你。本文还有配套的精品资源点击获取 扩展卡尔曼滤波EKF：非线性系统状态估计与机器人定位导航
简介这份资源围绕扩展卡尔曼滤波EKF展开面向从事惯性导航、姿态估计与多传感器融合的开发者及研究生重点解决9维IMU在姿态跟踪中的滤波实现问题。内容参考《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》论文给出基于EKF-IMU与ESKF-IMU两套算法的实现代码便于读者对照论文理解状态方程、观测模型与误差状态更新的差异。压缩包为zip格式整体约2.23MB文件类型以源码与工程文件为主可直接用于算法验证与二次开发。目前已有441人学习下载适合希望从理论推导过渡到工程落地、需要现成参考实现来调试姿态解算效果的读者也可作为课程设计或科研项目的起步素材。1. 扩展卡尔曼滤波一个让机器人不跑偏的“导航大脑”你有没有想过扫地机器人在你家客厅里转来转去怎么就知道自己现在在哪儿、下一步该往哪走它没有眼睛轮子还会打滑可它就是能一圈圈把地扫干净。这背后有个东西在默默干活叫扩展卡尔曼滤波英文缩写 EKF。说白了它就是一个“猜位置”的数学工具专门对付那些“不太听话”的系统——比如轮子会打滑、传感器有噪声、运动规律不是一条直线。普通卡尔曼滤波只能处理线性系统就像只能走直路而现实世界大多是弯的EKF 就是那个允许你走弯路的升级版。搞懂它你就明白为什么无人机能悬停、手机能定位、自动驾驶能跟车——这些场景里EKF 几乎无处不在。2. 为什么需要 EKF从“只能走直线”到“允许拐弯”2.1 卡尔曼滤波的局限线性假设太苛刻卡尔曼滤波诞生在上世纪六十年代一开始是为了解决阿波罗登月里的导航问题。它的核心思想很朴素你有一个“预测”还有一个“测量”两者都不准那就按各自的不确定度加权平均一下得到一个更准的估计。这个思路在线性系统里是最优的数学上漂亮得没话说。但问题来了什么叫线性系统打个比方你开车油门踩多少速度就涨多少方向打多少车头就转多少而且这个关系是成比例的、可叠加的。可现实里你踩油门速度不是线性涨的风阻是平方关系你打方向车走的轨迹是圆弧角度一变位置变化就不是简单乘法了。这些“弯的”关系卡尔曼滤波处理不了硬套上去就会翻车。注意很多初学者以为卡尔曼滤波就是一个公式背下来就行。其实它是一套“预测-更新”的递推框架线性假设是它的命门。2.2 EKF 的解法用“切线”把弯的掰直EKF 的做法很聪明既然整个曲线我搞不定那我就在当前估计点附近用一条切线去近似它。这条切线就是“线性化”。数学上叫一阶泰勒展开你不用记这个名字只需要知道EKF 在每个时刻都把非线性的运动模型和观测模型在当前估计值附近“掰直”成线性的然后再套用卡尔曼滤波的那套加权平均。这就像你要测量一个弯曲山坡的坡度你不可能一次测完整座山但你可以站在当前位置用一块小直板贴住地面量出这一点的坡度。EKF 就是不断在移动中做这件事每一步都重新“贴板子”所以它能跟着系统走。代价是什么线性化会引入误差。如果系统弯得太厉害或者你估计的点离真实值太远切线就偏得离谱EKF 可能发散——也就是越估越错最后彻底跑飞。这也是为什么后来有人搞出了无迹卡尔曼滤波UKF和粒子滤波它们不用线性化但计算量更大。EKF 的定位就是在精度和计算量之间找一个平衡点够用、快、好实现。2.3 一个生活化类比蒙眼找路想象你被蒙上眼睛站在一个广场上手里有个 GPS 告诉你大概位置但误差有十米。你每走一步心里默数步数和方向估算自己移动了多少。GPS 读数来了你就跟自己的估算对比一下如果 GPS 说你在东边你心里估算也在东边那就信自己多一点如果 GPS 突然跳到西边你心里估算没动那就怀疑 GPS 抽风了少信它一点。EKF 干的就是这件事只不过它用数学严格定义了“信谁多一点”——用协方差矩阵表示不确定度。你走得越久自己估算的不确定度越大就更信 GPSGPS 信号差的时候不确定度大就更信自己的推算。整个过程是递归的不需要存历史数据每一步只依赖上一步的估计和当前测量所以特别适合跑在单片机上。3. EKF 的核心零件预测、更新与雅可比矩阵3.1 预测步用运动模型往前推EKF 每个循环分两步第一步叫预测。你有一个状态向量比如机器人的位置 x、y 和朝向角 θ还有速度 v 和角速度 ω。根据上一时刻的状态你用运动学公式推算这一时刻的状态。如果是非线性函数 f就直接代进去算。同时你还要预测不确定度。不确定度用一个协方差矩阵 P 表示它描述了你对每个状态量的信心。预测时P 会变大因为推得越远越不准。这里的关键是非线性函数 f 不能直接乘 P需要先求它的雅可比矩阵 F。雅可比矩阵就是 f 对每个状态量的偏导数组成的矩阵它告诉你在当前点状态微小变化会如何影响下一步。说白了它就是“局部斜率”的多维版本。import numpy as np def predict(x, P, u, dt): # x: 状态向量 [x, y, theta, v, omega] # u: 控制输入这里假设为0实际可含加速度 # dt: 时间步长 theta x[2] v x[3] omega x[4] # 非线性运动模型 if abs(omega) 1e-6: x_new x[0] (v/omega) * (np.sin(theta omega*dt) - np.sin(theta)) y_new x[1] (v/omega) * (-np.cos(theta omega*dt) np.cos(theta)) else: x_new x[0] v * np.cos(theta) * dt y_new x[1] v * np.sin(theta) * dt theta_new theta omega * dt x_pred np.array([x_new, y_new, theta_new, v, omega]) # 雅可比矩阵 F对状态量求偏导 F np.eye(5) if abs(omega) 1e-6: F[0,2] (v/omega) * (np.cos(theta omega*dt) - np.cos(theta)) F[0,3] (np.sin(theta omega*dt) - np.sin(theta)) / omega F[0,4] (v/omega**2) * (np.sin(theta) - np.sin(theta omega*dt)) (v*dt/omega) * np.cos(theta omega*dt) F[1,2] (v/omega) * (np.sin(theta omega*dt) - np.sin(theta)) F[1,3] (-np.cos(theta omega*dt) np.cos(theta)) / omega F[1,4] (v/omega**2) * (np.cos(theta omega*dt) - np.cos(theta)) (v*dt/omega) * np.sin(theta omega*dt) else: F[0,2] -v * np.sin(theta) * dt F[0,3] np.cos(theta) * dt F[1,2] v * np.cos(theta) * dt F[1,3] np.sin(theta) * dt F[2,4] dt # 过程噪声协方差 Q需要根据实际调 Q np.diag([0.1, 0.1, 0.01, 0.1, 0.1]) P_pred F P F.T Q return x_pred, P_pred上面这段代码里F就是雅可比矩阵Q是过程噪声表示你对运动模型有多不信任。Q调大滤波器更信测量调小更信预测。这个参数没有标准答案得根据轮子打滑程度、地面摩擦来试。3.2 更新步用测量修正预测预测完你有了一个先验估计。接下来传感器来数据了比如激光雷达测到机器人离墙 1.2 米或者 GPS 给出经纬度。你把预测的状态代入观测方程 h算出“如果我在预测的位置传感器应该看到什么”。然后拿这个预测的观测值和实际观测值相减得到残差。残差乘以卡尔曼增益 K就得到修正量。K 的大小取决于预测不确定度 P_pred 和测量噪声 R 的比值。P_pred 大、R 小K 就大更信测量反之更信预测。更新完P 会变小因为融合了新的信息信心增加了。观测方程 h 通常也是非线性的比如从位置算距离和角度涉及平方和开方、反正切。所以更新步同样需要 h 的雅可比矩阵 H。H 的每一行对应一个观测量每一列对应一个状态量元素是偏导数。def update(x_pred, P_pred, z, R): # z: 实际观测 [range, bearing] # R: 测量噪声协方差 def h(x): px, py x[0], x[1] r np.sqrt(px**2 py**2) b np.arctan2(py, px) return np.array([r, b]) # 雅可比矩阵 H px, py x_pred[0], x_pred[1] r np.sqrt(px**2 py**2) H np.zeros((2, 5)) H[0,0] px / r H[0,1] py / r H[1,0] -py / (r**2) H[1,1] px / (r**2) z_pred h(x_pred) y z - z_pred # 角度残差归一化到 [-pi, pi] y[1] (y[1] np.pi) % (2*np.pi) - np.pi S H P_pred H.T R K P_pred H.T np.linalg.inv(S) x_new x_pred K y P_new (np.eye(5) - K H) P_pred return x_new, P_new这段代码里H是观测雅可比R是测量噪声。角度残差归一化那一步很容易被忽略但如果不做角度从 179 度跳到 -179 度时残差会变成 358 度滤波器直接懵掉。这是血泪经验新手必踩。3.3 雅可比矩阵EKF 的“方向盘”雅可比矩阵在 EKF 里扮演的角色就像方向盘在车里的角色。它不直接决定你去哪但它决定了你对“当前该往哪修正”的判断。如果雅可比算错了滤波器就会往错误方向修正轻则收敛慢重则发散。很多人第一次写 EKF运动模型和观测模型都写对了但结果就是不对最后发现是雅可比求导漏了一项或者符号搞反了。建议的做法是先用数值差分验证雅可比。比如对每个状态量加一个小扰动 ε算函数值变化再除以 ε跟解析雅可比对比。如果误差在 1e-6 量级说明推导正确。提示数值差分验证雅可比是调试 EKF 最有效的手段没有之一。花十分钟写个验证脚本能省你十小时抓头发。4. 易混概念EKF、KF、UKF、粒子滤波到底怎么分4.1 卡尔曼滤波KF与 EKF线性与非线性KF 和 EKF 最核心的区别就一个系统是不是线性的。KF 要求运动模型和观测模型都是矩阵乘法形式x_{k1} A x_k B u_kz_k C x_k。EKF 允许 f 和 h 是任意非线性函数代价是每步要算雅可比而且只是局部近似。什么时候用 KF如果你的系统本身就是线性的比如匀速直线运动、电路里的电压电流关系那就用 KF计算量小而且是最优估计。什么时候用 EKF只要模型里有三角函数、平方、开方、指数基本就得 EKF。但注意EKF 不是全局最优只是局部近似初值给得太离谱可能不收敛。4.2 EKF 与 UKF线性化 vs 采样UKF 全称无迹卡尔曼滤波它的思路跟 EKF 完全不同。EKF 是把非线性函数线性化UKF 是选一组确定性的采样点叫 sigma 点把这些点代入非线性函数算变换后的均值和协方差。它不需要求雅可比而且对强非线性系统往往比 EKF 更准。那为什么还用 EKF因为 UKF 计算量更大尤其是状态维度高的时候sigma 点数量是 2n1n 是状态维数。如果 n20就要算 41 个点每个点都要过一遍非线性函数。EKF 只需要算一个雅可比矩阵虽然求导麻烦但算起来快。所以嵌入式系统里 EKF 仍然很常见UKF 多用在精度要求高、算力也够的场景。4.3 EKF 与粒子滤波高斯假设的边界粒子滤波完全不假设分布是高斯它用一堆随机样本粒子来表示概率分布能处理多峰、非高斯的情况。比如机器人被绑架到另一个房间全局定位问题粒子滤波就能处理EKF 直接歇菜因为它只有一个高斯峰没法同时表示“我可能在 A 房间也可能在 B 房间”。但粒子滤波计算量巨大粒子少了精度不够粒子多了跑不动。而且它有随机性每次跑结果可能略有不同。EKF 是确定性的给定初值和参数每次结果一样便于调试和复现。所以选型逻辑是如果系统基本是单峰、近似高斯EKF 够用且高效如果多峰、强非线性再考虑粒子滤波。4.4 常见误区EKF 不是“更准的 KF”很多人以为 EKF 是 KF 的升级版精度一定更高。这是个误解。EKF 的精度取决于线性化误差。如果系统非线性很强或者工作点离真实值很远EKF 可能比 KF 还差甚至发散。KF 在线性系统里是最优的EKF 在非线性系统里只是“近似最优”而且这个近似好不好得看具体问题。另一个误区是“Q 和 R 随便设”。Q 是过程噪声R 是测量噪声它们直接决定卡尔曼增益影响滤波器的“性格”。Q 设太小滤波器太自信测量来了也不怎么修正响应慢Q 设太大滤波器太敏感测量噪声全放进来输出抖动。调 Q 和 R 是 EKF 实战里最耗时间的环节没有之一。5. 什么时候用 EKF什么时候别用一个具体例子假设你在做一个模拟项目让一个小车在走廊里沿着墙走车上有一个轮速计和一个激光雷达。轮速计给出速度但轮子会打滑激光雷达测距但偶尔有反射噪声。你想估计小车的位置和朝向。这个场景就是 EKF 的典型用武之地。状态量是 x、y、θ运动模型是非线性的因为 θ 在三角函数里观测模型也是非线性的距离和角度。EKF 能跑在单片机上实时性没问题。你需要做的就是写运动模型、写观测模型、求雅可比、调 Q 和 R。但如果你把场景换成小车在整栋楼里找自己可能在任何楼层任何房间初始位置完全未知。这时候 EKF 就不合适了因为它的高斯假设只能表示一个峰没法同时覆盖多个可能位置。你应该用粒子滤波或者多假设跟踪。再换一个场景小车在一条笔直的铁轨上跑轮速计测速度GPS 测位置。这就是线性系统直接用 KF没必要上 EKF。上 EKF 反而多算雅可比浪费算力还可能因为线性化引入不必要的误差。所以判断标准很清晰第一系统是不是非线性的不是就用 KF。第二分布是不是近似单峰高斯不是就考虑粒子滤波。第三算力够不够够且精度要求高可以试 UKF不够就 EKF。这三个问题问完选型基本就定了。我自己的习惯是每次拿到一个新系统先花半小时把运动模型和观测模型写成代码用数值差分验证雅可比然后跑一段离线数据看残差是不是零均值白噪声。如果残差有偏或者自相关说明模型有问题或者 Q、R 没调好。这个流程走一遍能避开大部分翻车现场。从那以后我每次上 EKF 之前都强制走一遍这个检查再也没出现过滤波器发散到外太空的情况。希望帮到你。本文还有配套的精品资源点击获取