简介面向惯性导航、传感器融合与姿态估计方向的学习者和工程开发者这份压缩包围绕九维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 - KH) P_pred (I - KH).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 点」把每个点分别通过真实的非线性函数传播一遍再用传播后的点重新拟合均值和协方差。前者是一阶精度需要你手推雅可比后者至少二阶精度不需要求导但每次要跑 2n1 次函数调用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(θ)最终整个滤波器一起漂。这种时候更好的做法是把θ从状态里拿掉直接用相邻两帧位置差分推算朝向作为控制量喂进去。另一个常见做法是误差状态 EKFerror-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̂⁻。这是最省事的做法也是绝大多数教材和开源实现的做法。但这里有个逻辑上的别扭更新步明明是想「用观测修正状态」可它却是在修正之前的那个点上做线性化。如果观测带来的修正量很大那么真正的最优点和线性化点相去甚远一阶近似就名不副实了。迭代 EKFIEKF就是针对这个问题的把更新步做成一个小循环每次用刚算出来的新状态重新线性化、重新算增益迭代几次直到状态不再明显变化。效果上它相当于在做一个局部的高斯-牛顿优化对强非线性观测尤其有效。代价是计算量翻几倍而且要处理迭代不收敛的情况。我的经验是如果单次更新的修正量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 每次要做 2n1 次非线性函数调用。在算力吃紧的板子上EKF 往往是唯一能塞进去的选择。第三类是系统里已经有可信的解析模型。如果你的运动学和观测模型是从物理原理推出来的导数能一项一项写清楚那 EKF 用起来会很顺雅可比是「顺便就写出来了」不是额外的负担。第四类是作为基线。上新算法之前先搭一个 EKF 跑通把新息序列、NEES 这些指标记录下来后面换 UKF 或者因子图优化时就有了一个明确的对比对象。EKF 实现简单、行为可解释作为基线非常合适。6.2 三种情况建议换方案非线性强到一阶项撑不住的时候。判断标准不是「函数里有没有三角函数」而是展开点附近的曲率有多大。一个实用的自检在当前估计点附近随机采一批样本用真实函数传播一遍再用一阶近似传播一遍比较两者的均值之差。如果这个差值和状态不确定度√diag(P)在同一个量级说明线性化误差已经开始主导EKF 给出的P会偏小滤波器会过度自信。这种情况下换 UKF 通常立竿见影。函数写不出导数的时候。比如观测模型是一张分段线性插值表、或者内部含大量if-else分支的逻辑。硬要上 EKF 就只能用数值差分近似雅可比每次迭代多花好几倍的函数调用精度还不如解析求导。UKF 天然不需要导数这类场合优势明显。后验分布根本不是单峰的时候。卡尔曼家族的所有成员——包括 EKF 和 UKF——都强行用一个高斯分布去描述信念。如果真实情况是多峰的比如机器人在一条长走廊里它可能在这一头也可能在那一头两个位置都说得通高斯根本表达不了。这种时候只能上粒子滤波或者用多假设跟踪让多个高斯并存。你的情况EKFUKF粒子滤波弱非线性、能解析求导、状态维度 15首选可用略贵大材小用强非线性一阶近似误差明显容易过度自信更稳推荐最稳但最慢函数不可导、含查表或分支只能数值差分不划算天然适配适配但慢后验多峰、非高斯表达不了表达不了唯一选择算力极紧、周期在微秒级唯一可行通常吃不消基本别想这张表不是绝对判据但能帮你快速排除明显不合适的方向。有一点必须提醒EKF 最容易出问题的时刻往往不是它跑得最差的时候而是它看起来跑得很好的时候。因为线性化误差会系统性地让P偏小滤波器越来越相信自己的预测越来越不理观测输出曲线平滑得漂亮实际误差却在缓慢累积。等误差大到肉眼可见通常已经积累了几十秒甚至几分钟。从那以后我每次启动一个新的 EKF都会强制走一遍三件事拿静止数据核对R的量级用数值求导交叉验证雅可比再跑一段真实数据看 NEES 有没有偏离状态维度。这三步加起来不到半小时却能挡住绝大多数后面要靠几天才能查出来的问题。希望帮到你。本文还有配套的精品资源点击获取