简介：面向惯性导航、传感器融合与姿态估计方向的学习者和工程开发者，这份压缩包围绕九维IMU定向跟踪问题，整理了基于双阶段卡尔曼滤波论文的复现资料，并包含EKF-IMU与ESKF-IMU两类算法的实现代码，可用于理解扩展卡尔曼滤波在真实传感器数据处理中的建模、状态更新与误差补偿思路。压缩包整体约2.23MB，文件总数记录为0、主要文件类型明细暂无数据，读者可将其视为算法复现与对照学习的代码资料，具体目录以下载后实际内容为准。已有441人学习或下载，适合需要从论文公式过渡到工程实现、并对比EKF与ESKF滤波效果的读者参考。资源重点是双阶段滤波的落地实现与IMU姿态跟踪流程，便于在课程设计、毕业设计或算法验证中借鉴其状态量定义、滤波框架和实验组织方式。

1. 到底什么是扩展卡尔曼滤波：一次「蒙眼估位置」的困惑

蒙着眼睛在房间里走，你一边数步子猜自己在哪，一边偶尔伸手摸到墙，得到一个模模糊糊的距离。两个信息都不准，但你脑子里那个「最可能的位置」会不停被两边拉扯着修正：脚步说往前走了三步，手掌说离墙只剩半米，于是你把估计往回拽一点。这套「先预测一步，再用观测纠正一次」的循环，就是卡尔曼滤波。

而扩展卡尔曼滤波（Extended Kalman Filter，圈内几乎都直接叫EKF）是它的变种。差别只在一个地方：原版卡尔曼滤波假设「走一步就是一米」这种规规矩矩的直线关系，一旦你的动作里出现转弯、出现开方、出现三角函数，它就会直接失效；EKF 就是为这种「弯的」情况准备的替代方案。

值得花十分钟搞懂它，是因为无人机姿态、机器人定位、雷达跟踪、组合导航这些场景里，模型几乎从来没有「线性」过。EKF 做的事情很朴素：把弯的东西在脚下那一小段掰直，然后继续用老办法算。

2. 为什么需要 EKF：线性假设是怎么一步步崩掉的

2.1 标准卡尔曼滤波活在一个「直线世界」里

卡尔曼滤波能成立，靠的是三条相当理想化的前提：状态怎么演变是线性的，观测怎么得到也是线性的，所有噪声都服从零均值的高斯分布。翻译成人话就是——走一步固定是一米，传感器读数固定等于真实值乘个系数，误差像一口钟一样对称地散在真值两侧。

在这三条都满足的时候，卡尔曼滤波不只是「能用」，它在数学上是最优的：给定同样的历史信息，没有任何别的算法能给出更小的平均平方误差。这也是它几十年经久不衰的原因——它不是一堆经验规则的拼凑，而是有明确最优性保证的推导结果。

麻烦在于，现实里的物理量很少这么听话。一辆小车在地面上跑，只要它带一点转向，位置更新里就必然出现cos(θ)和sin(θ)；一个雷达测距，读数必然是两点坐标的平方和再开根号；一块电池的电压和剩余电量之间，是一条弯弯曲曲的经验曲线。这些都不是「乘个系数」能描述的，线性假设在这里不是「略微不准」，而是从根上就不成立。

一旦假设崩了，卡尔曼滤波的整套最优性证明就全部作废。你还是可以硬套那五个公式，程序也不会报错，但输出的估计会越来越偏，协方差会给出一个「我很确定」的假象，这就是工程上说的滤波器发散——它不崩溃，它只是安静地骗你。

2.2 非线性从哪来：三个绕不开的场景

第一个来源是坐标与几何。最典型的是移动机器人的状态[x, y, θ]，控制量是速度v和角速度ω，走过dt秒之后：

x' = x + v·cos(θ)·dt y' = y + v·sin(θ)·dt θ' = θ + ω·dt

前两行里的cos(θ)、sin(θ)就是非线性的入口。你可以把状态换成别的表示，但只要涉及旋转，非线性就躲不掉。

第二个来源是观测方程。雷达测距公式r = √((bx - x)² + (by - y)²)里，待估的x、y被套在平方和开方里面。你没法把它整理成r = a·x + b·y的形式，无论怎么变形都做不到。

第三个来源是经验模型本身。电池管理里，开路电压和荷电状态之间是一条缓慢爬升的 S 形曲线；某些传感器的输出要经过分段标定表插值。这些关系连解析表达式都没有，只有一条曲线或者一张表。

三个场景的共同点是：函数f和h都不是直线。而卡尔曼滤波的五个公式，本质上是在对直线做加权平均。

2.3 一阶泰勒展开：把弯的掰直的那把尺子

EKF 的想法可以用一句大白话概括：既然整体是弯的，那就只看脚下这一小段，把它当成直的。

这在数学上叫一阶泰勒展开。把非线性函数f在当前估计点x̂附近展开，丢掉二阶及以上的项：

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

这里F是f在x̂处的导数（多维情况下是一个矩阵）。这个式子的意思很直白：f(x̂)是「你现在认为的答案经过模型之后落在哪」，F·(x - x̂)是「如果真实状态比你以为的偏了一点点，输出会偏多少」。

打个比方，地球是圆的，但你家门口那条街你一定当平的来铺地砖，误差小到可以忽略。EKF 干的就是这件事：它不假装世界是平的，它只是在你现在站着的那一小块地方，把世界当平的算。走得越远，这块「局部平面」和真实曲面的差距就越大，这也埋下了后面所有问题的种子。

需要强调的是，这个「掰直」不是随便挑个点。EKF 规定：预测时在当前估计x̂处展开，更新时在预测出来的x̂⁻处展开。选哪个点，直接决定误差有多大，后面第 5 章会专门讲这个坑。

2.4 雅可比矩阵：一张局部灵敏度表

F和H这两个矩阵有个专门的名字——雅可比矩阵（Jacobian）。它的每一个元素就是一个偏导数∂fᵢ/∂xⱼ，含义是「第 j 个状态量变动一丁点，第 i 个输出量会跟着动多少」。

把它想成一张汇率表：你手里有位置、速度、朝向好几种「货币」，想知道它们各自换成「观测值」这种货币值多少。雅可比就是当下的牌价，而且牌价会随你站的位置变化——在房间这头和那头，距离对 x 坐标的敏感度完全不同。

以一个二维平面上的测距为例：

import numpy as np BEACON = np.array([5.0, 3.0]) # 信标坐标，事先标定好的常量 def h(state): """观测函数：返回机器人到信标的距离，1 维输出""" x, y, _ = state return np.array([np.hypot(x - BEACON[0], y - BEACON[1])]) def jacobian_h(state): """观测雅可比：1x3，对 x、y、theta 三个状态分别求偏导""" x, y, _ = state dx, dy = x - BEACON[0], y - BEACON[1] r = max(np.hypot(dx, dy), 1e-6) # 加下限，避免机器人和信标重合时除零 return np.array([[dx / r, dy / r, 0.0]]) # 第三列为 0：距离与朝向无关

逐项说一下。h的输出是 1 维的（一个距离值），状态是 3 维的（x, y, θ），所以H的形状是 1×3。前两个元素dx/r、dy/r其实是「从信标指向机器人的单位向量」，几何意义很清楚：机器人在信标正东方时，x 方向动一点距离变化最明显，y 方向几乎没影响。第三列写 0，因为机器人原地转圈并不会改变它到信标的距离。

1e-6那行是个不起眼但很要命的细节。如果某次迭代中机器人恰好和信标重合，r变成 0，这一除就会产生inf或者nan，然后顺着矩阵乘法污染整个协方差，几分钟后你看到的就是一条疯掉的曲线。工程代码里这类保护比公式本身更重要。

3. EKF 是怎么跑起来的：预测与更新的两步循环

3.1 三样东西：状态、协方差、两个模型

理解 EKF 只需要盯住三样东西。

状态向量x，是你想估计但测不准的量。比如小车的位置和朝向[x, y, θ]，比如无人机的姿态四元数，比如电池的荷电状态。

协方差矩阵P，是对「我有多不确定」的量化。对角线上的元素是各个状态量的方差，非对角线是它们之间的相关性——比如「如果我高估了 x，很可能也高估了 y」，这种耦合就写在非对角线上。P是 EKF 里信息量最大的东西，也是最容易被新手忽略的东西。

两个模型。运动模型f(x, u)描述「在控制量 u 的作用下状态怎么变」，观测模型h(x)描述「状态怎么变成传感器读数」。这两个函数都不需要线性，但都必须能求导——至少得能写出雅可比。

再加上两个噪声矩阵：Q是过程噪声，代表模型本身有多不靠谱；R是观测噪声，代表传感器有多不靠谱。这两个矩阵不是从数据里自动学出来的，是你要手动填的，填得好不好直接决定滤波器是稳如老狗还是当场翻车。

3.2 预测步：把不确定性吹大

预测步做的事情，用一句话说就是「按模型往前推一步，同时承认自己变得更不确定了」。

状态往前推是直接的：x̂⁻ = f(x̂, u)。非线性函数照原样用，不做任何线性化——这是 EKF 的一个容易混淆的点，很多人以为 EKF 连状态传播都要近似，其实不是，它就是老老实实调一次f。

协方差往前推才需要线性化：P⁻ = F·P·Fᵀ + Q。这里的F是运动模型在x̂处的雅可比。

看这个式子的结构就很有意思。F·P·Fᵀ是「把原来的不确定性按模型的敏感度传下去」：如果某个方向上模型放大误差（F的元素绝对值大于 1），那个方向的不确定性就被放大；如果模型压缩误差，不确定性就缩小。然后+ Q兜底，代表「不管怎样，模型本身还有一份噪声」。

所以预测步的净效果通常是不确定性变大。这符合直觉：你对未来的判断总是不如对现在清楚。Q越大，P膨胀得越快，滤波器就越倾向于相信传感器，而不是相信自己。

3.3 更新步：卡尔曼增益其实是一场加权平均

更新步的核心是那个叫卡尔曼增益K的矩阵。它的作用可以完全脱离矩阵语言来理解：在两个都不太准的估计之间做加权平均，谁更不靠谱就少听谁的。

公式是这样的：

S = H·P⁻·Hᵀ + R # 观测预测的总不确定度 K = P⁻·Hᵀ·S⁻¹ # 增益 x̂ = x̂⁻ + K·(z - h(x̂⁻)) # 用残差修正状态 P = (I - K·H)·P⁻ # 不确定性收缩

z - h(x̂⁻)叫新息（innovation），意思是「传感器实际读到的东西，和模型预测它会读到的东西，差了多少」。如果这个差是零，说明模型预测和观测完全一致，x̂就不用改；差得越多，修正越猛，而修正多猛由K决定。

K的取值很有讲究。当R很大（传感器很吵），S变大，K就变小，滤波器基本不理观测，继续信自己；当P⁻很大（预测很虚），K就变大，滤波器几乎照着观测改。极端情况下K趋近于单位矩阵，那就是「完全相信传感器」；K趋近于零矩阵，那就是「完全不听传感器」。

这也是 EKF 和它的近亲们共有的精髓：不确定性是会被自动调配的资源。你不需要写一堆if来判断该信谁，P和R的相对大小自动完成了这个权衡。

最后那行P = (I - K·H)·P⁻常被忽略，但它才是让滤波器「越用越准」的关键。每次更新之后，通过观测获得的信息会削减不确定性，P的对角线元素变小，下一次的K就自动变小，滤波器对观测的依赖随之下降。如果这行写错了，P会一直保持很大，导致滤波器永远过度依赖观测，稍微来个离群点就被带偏。

3.4 一段能读懂的伪代码与参数说明

把两步合起来，一个最小可用的 EKF 迭代长这样：

import numpy as np def ekf_step(x, P, u, z, dt, Q, R): # ---------- 预测 ---------- x_pred = f(x, u, dt) # 直接用非线性模型推状态 F = jacobian_f(x, u, dt) # 在"当前估计"处求运动雅可比 P_pred = F @ P @ F.T + Q # 不确定性传播并加上过程噪声 # ---------- 更新 ---------- z_pred = h(x_pred) # 模型预测的观测值 H = jacobian_h(x_pred) # 在"预测点"处求观测雅可比 S = H @ P_pred @ H.T + R # 新息的协方差 K = P_pred @ H.T @ np.linalg.inv(S) # 卡尔曼增益，实际可用 solve 代替 inv x_new = x_pred + K @ (z - z_pred) # 用新息修正状态 P_new = (np.eye(len(x)) - K @ H) @ P_pred # 更新不确定性 return x_new, P_new

参数逐个数一下。x是上一时刻的后验状态，P是对应的后验协方差；u是这一时刻的控制量（速度、角速度之类）；z是这一时刻拿到的实际观测向量；dt是时间间隔，通常由时间戳算出来而不是写死；Q和R是常量矩阵，一般按经验设成对角阵。

几个工程上的补充。np.linalg.inv(S)在S条件数很差的时候会给出离谱的结果，实践中更稳的写法是np.linalg.solve(S.T, (P_pred @ H.T).T).T，用解线性方程组代替求逆。f和jacobian_f必须是严格对应的一对，模型改了导数没改，是最常见的翻车原因之一，而且这种错误非常隐蔽——滤波器还能跑，只是慢慢偏。

还有一点，P_new那个式子在对角线数值很大或者R很小时，浮点误差会让P丢掉对称性。稳妥的做法是用 Joseph 形式：P_new = (I - K@H) @ P_pred @ (I - K@H).T + K @ R @ K.T，代价是多两次矩阵乘法，换来的是无论K怎么算，P都保持对称半正定。

4. 常见误区与易混概念：EKF、KF、UKF、粒子滤波到底差在哪

4.1 误区一：EKF 是「更准的卡尔曼滤波」

易混点：很多人第一次看到「扩展」两个字，直觉上认为这是原版卡尔曼滤波的升级版，好比从标准版升级到旗舰版，精度一律更高。

为什么混：名字里带「扩展」，而且它确实是在原版的基础上加的，读起来就像是改良。加上 EKF 的应用场景（自动驾驶、机器人）听起来比课本上的卡尔曼滤波「高级」得多，更容易强化这个印象。

怎么区分：看模型。如果系统是线性的，f和h都是矩阵乘法，那么雅可比就是常数矩阵，EKF 会退化成标准卡尔曼滤波——两者算出来的结果一模一样，一个数都不会差。所以在这种问题上，EKF 不是「更准」，是「完全等价」。而如果系统是非线性的但非线性很强，EKF 因为只保留了一阶项，精度反而可能不如某些近似方法。一句话：EKF 不是进阶版，是线性假设失效时的补丁，补丁有它的代价，代价就是引入了线性化误差。

4.2 误区二：雅可比算错了也能跑，就说明没问题

易混点：程序跑通了、曲线画出来了、滤波器没崩，于是认为雅可比推导是对的。

为什么混：EKF 对雅可比误差的容忍度相当高。雅可比写错一点点，滤波器短期内的行为和正确的几乎一样，因为真正决定输出的是状态传播和观测残差，雅可比只影响增益。而增益对结果的影响是渐进的，不会立刻表现成一条诡异的曲线。

怎么区分：用数值求导做交叉验证。对每个状态分量施加一个很小的扰动ε，用(f(x+ε) - f(x-ε)) / (2ε)算一个近似导数，和解析式比对，相对误差超过 1e-4 就要警惕。这个检查花两分钟，能省掉后面几天的怀疑人生。另外要看的指标是「新息是否白」。正常的滤波器，z - h(x̂⁻)应该是一个均值为零、分布固定的序列；如果新息里有明显的趋势或者周期成分，说明模型或者雅可比有系统性错误，这时候光调Q、R是掩盖问题，不是解决问题。

4.3 误区三：EKF 和 UKF 只是写法不同，随便挑一个

易混点：既然两个都能处理非线性，那就当两种实现风格，谁代码短用谁。

为什么混：两者输入输出完全一样——进去状态、协方差、观测，出来修正后的状态、协方差，接口一致，用起来感觉就是可替换的组件。

怎么区分：关键在它们对「非线性」的处理方式根本不同。EKF 是在一个点上做解析求导，把函数掰直；**UKF（无迹卡尔曼滤波）**不掰直，而是挑一小组「sigma 点」，把每个点分别通过真实的非线性函数传播一遍，再用传播后的点重新拟合均值和协方差。前者是一阶精度，需要你手推雅可比；后者至少二阶精度，不需要求导，但每次要跑 2n+1 次函数调用（n 是状态维度）。

所以在状态维度低、函数能求导、非线性温和的场合，EKF 又便宜又够用。而在函数写不出导数（比如查表插值、含大量分支的逻辑）、或者非线性明显到一阶近似撑不住的时候，UKF 更合适。代价是维度高时计算量涨得很快，因为函数调用次数随维度线性增长。

4.4 误区四：Q调大一点更稳，R调小一点更信观测

易混点：把Q和R当成两个「旋钮」，哪个方向效果不好就往那个方向拧。

为什么混：这两个矩阵确实会显著改变输出曲线的形状，看起来就像调节灵敏度。而且调完之后曲线往往真能变好看一点，形成正反馈。

怎么区分：Q和R不是自由参数，它们有物理含义，而且它们的比值决定了卡尔曼增益的稳态值。把Q调大会让滤波器更相信观测，短期内跟踪更紧、滞后更小，看起来「更准」；但同时P会被持续推高，滤波器对自己的估计过分不确定，遇到一个离群观测就会大幅跳变。把R调小是同一个效果的另一条路径。

真正的判据是一致性，不是曲线好不好看。一个一致的滤波器，它报告的P应该和实际误差的统计量吻合——理论上，归一化估计误差平方（NEES）的期望值应该等于状态维度 n。如果你算出来 NEES 长期远大于 n，说明滤波器过度自信；远小于 n，说明它过度保守。这个检验比盯着曲线调参靠谱得多，也是从「玄学调参」走向「有依据调参」的分水岭。

5. 工程落地要点：状态量、Q 与 R、线性化点怎么定

5.1 状态量怎么选：少即是多

EKF 的状态向量不是越多越好。每多一个状态量，协方差矩阵就多一行一列，雅可比多一列，计算量按平方涨；更重要的是，每个不可观测的状态都会变成一个缓慢漂移的隐患。

判断一个量该不该放进状态，问三个问题。第一，它是否随时间演化，且我关心它的当前值？第二，是否有某个观测（哪怕是间接地）对它敏感？第三，它的演化模型我能不能写出来？

举个例子。做平面定位时，把[x, y, θ]放进状态是标准的。但如果传感器只有 GPS 而没有陀螺仪，θ的唯一信息来源是「位置变化的方向」，这在低速或者静止时几乎不可观测——此时θ会随着噪声随机游走，而它又反过来污染位置预测中的cos(θ)、sin(θ)，最终整个滤波器一起漂。这种时候更好的做法是把θ从状态里拿掉，直接用相邻两帧位置差分推算朝向，作为控制量喂进去。

另一个常见做法是误差状态 EKF（error-state EKF）：不直接估计姿态本身，而是估计「真实姿态相对于名义姿态的小偏差」。这样状态的量级始终很小，线性化点永远在零点附近，一阶近似的精度大幅提高。姿态估计领域大量使用这个变体，原因就在于此。

5.2 Q 与 R：调参的玄学与血泪经验

R相对好办一些，它对应传感器的实际噪声。拿静止的数据录一段，算方差，就是R的对角线初值。如果传感器在不同量程下噪声不同（比如远距离测距误差更大），可以让R随观测值变化，每次更新时重算，这在 EKF 框架里是允许的，只要保证R正定。

Q就麻烦得多，因为过程噪声往往没有直接可测的来源。它要同时承担三件事：真实的外部扰动、模型简化带来的误差、以及离散化引入的误差。这三者混在一起，就只能靠经验估。我踩过的坑是：一开始把Q设得极小（因为「我的模型很准」），结果滤波器对自己的预测过于自信，观测基本被忽略，等到误差积累到肉眼可见时已经拉不回来了。

一个实用的做法是从大到小试。先把Q设成一个明显偏大的值，让滤波器充分跟随观测，确认整体框架没问题；然后再逐步减小，观察新息序列什么时候开始出现明显的相关性——一旦新息不再像白噪声，说明Q已经太小，滤波器开始忽略本该重视的信息。这个边界点附近通常就是合适的取值。

还有一点容易被忽略：Q和R的相对比例比绝对大小更重要。把所有噪声统一乘 10，滤波器的行为基本不变，因为增益只依赖比值。所以不必纠结「Q到底该是 0.01 还是 0.02」，先把量级搞对。

5.3 线性化点选在哪：先验、后验还是反复迭代

标准 EKF 的线性化点有两个：预测时用上一时刻的后验x̂，更新时用预测出来的先验x̂⁻。这是最省事的做法，也是绝大多数教材和开源实现的做法。

但这里有个逻辑上的别扭：更新步明明是想「用观测修正状态」，可它却是在修正之前的那个点上做线性化。如果观测带来的修正量很大，那么真正的最优点和线性化点相去甚远，一阶近似就名不副实了。

迭代 EKF（IEKF）就是针对这个问题的：把更新步做成一个小循环，每次用刚算出来的新状态重新线性化、重新算增益，迭代几次直到状态不再明显变化。效果上，它相当于在做一个局部的高斯-牛顿优化，对强非线性观测尤其有效。代价是计算量翻几倍，而且要处理迭代不收敛的情况。

我的经验是：如果单次更新的修正量（K·新息的模）经常明显大于状态本身的不确定度√diag(P)，那就值得上 IEKF。反过来，如果修正量一直很小，标准 EKF 就够了，加迭代只是浪费算力。

5.4 数值稳定性：协方差为什么会炸

协方差矩阵在数学上必须对称且半正定，但在浮点运算里，这两个性质会被一点点侵蚀。几百上千次迭代之后，P可能变得不对称，甚至出现负的特征值——一旦P有负特征值，理论上它就不再是一个合法的协方差，后续的增益计算可能给出荒谬的结果，然后整个滤波器瞬间发散。

常见的三种崩法和对策如下。

第一种是求逆的数值问题。当S = H·P⁻·Hᵀ + R接近奇异（比如两个观测高度相关、或者R设得极小），直接调用求逆会放大误差。改用 Cholesky 分解或solve，能显著改善。

第二种是对称性丢失。(I - K·H)·P⁻这个形式在数值上不保证对称，误差累积后P会慢慢偏斜。解决办法是用 Joseph 形式：P = (I-KH)·P⁻·(I-KH)ᵀ + K·R·Kᵀ，它对任意K都保持对称半正定。慢一点，但稳。

第三种是协方差塌缩。P的某些对角元素变得极小（比如 1e-20），滤波器认为那些状态已经完全确定，从此再也不更新它们。这通常是因为Q里对应的项设成了 0，或者模型过于理想。给Q的对角线留一个极小的正数（比如 1e-9），等于给每个状态留一条「永远保留一点怀疑」的后路，就能避免这种情况。

这三种问题都不体现在公式里，只体现在跑了几万帧之后突然有一天结果完全不对。所以我现在的习惯是：在开发阶段就把P的最小特征值、S的条件数、新息的白噪声检验这几个量打印出来，做成一个监控面板——它就是这套系统的黑匣子，出事的时候能立刻定位是哪一环先出的问题。

6. 什么时候该用 EKF，什么时候最好别碰

6.1 四类适合上 EKF 的场景

第一类是弱到中等非线性，且函数可解析求导。这是 EKF 的主场。机器人在平面上的定位、无人机的姿态解算、组合导航里的位置速度融合，非线性来自三角函数和开方，在正常运行范围内局部近似得很好，一阶泰勒展开的残差是二阶小量，完全够用。这类系统的状态维度通常在 3 到 15 之间，EKF 一次迭代的耗时在微秒级，跑在单片机上都不成问题。

第二类是实时性有硬约束的嵌入式应用。控制周期 1 毫秒，中间要完成感知、融合、决策，留给滤波的时间只有几十微秒。EKF 的计算量是O(n³)（主要是矩阵乘法），而粒子滤波要跑几百上千个粒子，UKF 每次要做 2n+1 次非线性函数调用。在算力吃紧的板子上，EKF 往往是唯一能塞进去的选择。

第三类是系统里已经有可信的解析模型。如果你的运动学和观测模型是从物理原理推出来的，导数能一项一项写清楚，那 EKF 用起来会很顺，雅可比是「顺便就写出来了」，不是额外的负担。

第四类是作为基线。上新算法之前，先搭一个 EKF 跑通，把新息序列、NEES 这些指标记录下来，后面换 UKF 或者因子图优化时，就有了一个明确的对比对象。EKF 实现简单、行为可解释，作为基线非常合适。

6.2 三种情况建议换方案

非线性强到一阶项撑不住的时候。判断标准不是「函数里有没有三角函数」，而是展开点附近的曲率有多大。一个实用的自检：在当前估计点附近随机采一批样本，用真实函数传播一遍，再用一阶近似传播一遍，比较两者的均值之差。如果这个差值和状态不确定度√diag(P)在同一个量级，说明线性化误差已经开始主导，EKF 给出的P会偏小，滤波器会过度自信。这种情况下换 UKF 通常立竿见影。

函数写不出导数的时候。比如观测模型是一张分段线性插值表、或者内部含大量if-else分支的逻辑。硬要上 EKF 就只能用数值差分近似雅可比，每次迭代多花好几倍的函数调用，精度还不如解析求导。UKF 天然不需要导数，这类场合优势明显。

后验分布根本不是单峰的时候。卡尔曼家族的所有成员——包括 EKF 和 UKF——都强行用一个高斯分布去描述信念。如果真实情况是多峰的（比如机器人在一条长走廊里，它可能在这一头，也可能在那一头，两个位置都说得通），高斯根本表达不了。这种时候只能上粒子滤波，或者用多假设跟踪，让多个高斯并存。

这张表不是绝对判据，但能帮你快速排除明显不合适的方向。

有一点必须提醒：EKF 最容易出问题的时刻，往往不是它跑得最差的时候，而是它看起来跑得很好的时候。因为线性化误差会系统性地让P偏小，滤波器越来越相信自己的预测，越来越不理观测，输出曲线平滑得漂亮，实际误差却在缓慢累积。等误差大到肉眼可见，通常已经积累了几十秒甚至几分钟。

从那以后我每次启动一个新的 EKF，都会强制走一遍三件事：拿静止数据核对R的量级，用数值求导交叉验证雅可比，再跑一段真实数据看 NEES 有没有偏离状态维度。这三步加起来不到半小时，却能挡住绝大多数后面要靠几天才能查出来的问题。希望帮到你。

本文还有配套的精品资源，点击获取