简介:面向惯性导航、机器人姿态估计及传感器融合的算法研究与工程开发人员,这份资源以一篇9轴IMU方向跟踪的双级卡尔曼滤波论文为基础,完整提供了EKF-IMU与ESKF-IMU两套可运行的算法实现。压缩包体积约2.23MB,内容以源码和工程配置为主,包内文件明细未完整列出,但从算法描述看包含核心滤波代码、论文配套实验所需的参考实现。目前已有441人次学习/下载,适合作为姿态跟踪、惯性导航或组合导航项目起步阶段的算法参考。通过代码与实现文档,读者可以对照论文理解扩展卡尔曼滤波与误差状态卡尔曼滤波在9轴IMU数据融合中的状态建模、误差传播、观测更新与协方差修正策略;同时能快速掌握两类滤波器的工程化写法、初始化与调参要点、常见调试流程,以及从论文公式到可运行代码的落地思路,减少自行复现的时间成本。
1. 扩展卡尔曼滤波:非线性系统里,为什么EKF是最务实的“次优解”
很多人学完标准卡尔曼滤波,一接真实传感器数据就翻车:加速度计、陀螺仪、GPS 的输出全是非线性关系,五步公式根本套不进去。扩展卡尔曼滤波(EKF)的思路很直接——在每次递推时用雅可比矩阵把非线性函数在估计点处切一刀,近似成线性,再走卡尔曼的老路。它不是理论最优,却是工程上最常用的“次优解”。我拆过一份 EKF 实战源码包,里面从一维压力测距融到 IMU+GPS 双传感器定位,把公式、代码、调参踩坑全串起来了。适合做无人车定位、姿态解算、传感器融合的开发者,照着复现能少走几个月弯路。
2. 从线性卡尔曼到EKF:雅可比矩阵与五步递推的维度核对
2.1 线性卡尔曼的假设,EKF到底放开了什么
标准卡尔曼滤波隐藏了三个硬性假设:系统是线性的,过程噪声和观测噪声是零均值高斯白噪声,初始状态误差服从高斯分布。其中“线性”最致命。真实系统里,观测方程经常是距离、角度、三角函数,状态转移也可能带旋转矩阵、航向角,高斯的线性变换性质完全不成立。比如 GPS 输出的经纬度换算到平面坐标后,观测矩阵还算线性,但超声波测距电压与距离成反比、雷达量测是极坐标到直角坐标的转换,这些一进卡尔曼框架就全乱套。
EKF 的解决办法俗称为“局部线性化”:在每次递推的估计点处,对状态转移函数 f(x,u) 和观测函数 h(x) 做一阶泰勒展开,丢掉二阶以上项。比如非线性函数 h(x)=arctan(...),展开后变成关于 x 的线性近似式 h(x)≈h(x̂)+H(x−x̂),其中 H 是该点雅可比矩阵。这样高斯分布经过近似线性变换后依然保持高斯,卡尔曼框架就能继续跑。实际落地时,常见做法是保持较高的更新频率,让估计点始终贴着真实轨迹走,把线性化误差压小。我见过不少项目拿着 1Hz 的 GPS 直接喂给 EKF,结果线性化误差大到滤波来回震荡,后来把 IMU 预测频率提到 50Hz,问题立刻缓解。
代价是什么?线性化带来的截断误差会让估计不再是最优。EKF 不是全局最优估计,而是在“当前估计点附近”的局部最优。这也是为什么 EKF 在强非线性场景下不如 UKF,但在工程中,只要噪声不算大、采样频率够快,EKF 的表现足够稳。很多人纠结“最优性”,实际做系统时我更看重稳定性和算力开销,EKF 在这两项上的表现仍然是最平衡的。
2.2 雅可比矩阵怎么算:解析法、数值法与一个具体算例
雅可比矩阵是 EKF 所有公式里最容易出错的部分。F=∂f/∂x 是 n×n,H=∂h/∂x 是 m×n。求法有两种:解析法手工推导,数值法在估计点处做有限差分。解析法精确但费神,数值法通用但每次都要重新算一遍。我一般优先写解析法,因为数值法在高维系统里计算开销大,而且步长选不对会引入新的误差。不过第一次写完解析式,我通常会拿数值法做一次交叉验证,确保偏导没错。
以一个二维状态 x=[θ, ω] 为例,状态转移是 f(x)=[θ+ωdt, ω],这是线性的,F 直接是 [[1, dt],[0,1]]。但观测方程若取 h(x)=sin(θ),那么 H 就是 ∂h/∂x=[cosθ, 0]。这个例子虽小,却把“在估计点处取值”这件事表现得够清楚:H 矩阵里存的是 θ̂ 处的 cos 值,每次更新都要重新算。真实项目里常见的是航向角观测,h(x)=atan2(py,px),偏导里带着除法,稍不留神就会漏掉分母平方。
如果函数太复杂、解析法容易算错,可以退而用有限差分:
import numpy as np def numerical_jacobian(fun, x, u, dt, eps=1e-6): n = x.shape[0] F = np.zeros((n, n)) x0 = x.copy() for i in range(n): xp = x0.copy(); xp[i, 0] += eps xm = x0.copy(); xm[i, 0] -= eps fp = fun(xp, u, dt) fm = fun(xm, u, dt) F[:, i] = ((fp - fm) / (2 * eps)).ravel() return F这里 eps 一般取状态的千分之一量级,太小会因浮点误差失效,太大则偏离线性区。很多代码库里都有自动微分,但在嵌入式裸机上,解析法还是最可靠。注意输出里 F[:, i] 的维度是 n×1 展平,和 x 的 shape 有关,建议统一用列向量。
2.3 EKF五步递推与矩阵维度核对
EKF 把卡尔曼五步公式原样保留,只是把 A、H 换成了 F、H,并且状态与观测的传播都经过非线性函数。预测两步:
x̂_pred = f(x̂_prev, u) P_pred = F P_prev F^T + Q
更新三步:
S = H P_pred H^T + R K = P_pred H^T S^{-1} x̂_new = x̂_pred + K (z − h(x̂_pred)) P_new = (I − K H) P_pred
这里面 S 是 m×m 新息协方差,K 是 n×m 增益。写代码前先标好维度:状态 n 维、观测 m 维,F 是 n×n、Q 是 n×n、H 是 m×n、R 是 m×m、I 是 n×n。新手最容易把 H 写成 n×m 然后让 numpy 广播出一个错得离谱的矩阵。我之前在一次多传感器融合调试里发现 P 矩阵形状变成了 n×n×3,追了半天才发现是 H 的维度写反,导致广播机制创造了多余维度。
| 符号 | 含义 | 维度 | 常见初值 |
|---|---|---|---|
| x | 状态向量 | n×1 | 首个观测反推 |
| P | 状态协方差 | n×n | 对角阵,取观测噪声量级 |
| F | 状态转移雅可比 | n×n | 单位阵加 dt 项 |
| Q | 过程噪声协方差 | n×n | 对角阵,按控制输入噪声标定 |
| H | 观测雅可比 | m×n | 按观测方程求导 |
| R | 观测噪声协方差 | m×m | 传感器手册或实测方差 |
| K | 卡尔曼增益 | n×m | 无需初值 |
在写代码之前还应想清楚 Q 和 R 到底代表什么。Q 描述的是模型不确定度:比如你把加速度当成控制输入,但实际加速度有抖动,这部分不确定性就得塞进 Q。R 描述的是传感器噪声:GPS 标称精度 1 米,一般取 R=1^2=1。这两个矩阵的比例决定了滤波器在预测和观测之间更信谁。把 Q 调大,滤波器会更快跟上测量;把 R 调大,输出会更平滑但滞后更多。工程上先用传感器手册给 R,再通过残差统计反推 Q,别一上来就凭感觉乱填。
提示:P 更新写成 (I−KH)P_pred 在理论上和 Joseph 形式等价,但后者数值对称性更好。EKF 迭代次数多了之后,减法容易把 P 矩阵弄得不正定,Joseph 形式多花几次矩阵乘法,却能保住半正定性。这是我长期跑数据后养成的习惯,具体写法见第 4 章。
3. 手写EKF实战:从一维强非线性观测到IMU+GPS融合的Python实现
3.1 热身:一维EKF,观测方程是强非线性的
为了先跑通五步公式,用一个最简单的标量系统:状态是距离 d(单位 m),控制输入恒定,观测是超声波模块输出的电压值 v=K/d+噪声。电压与距离成反比,这是强非线性。用 Python 实现:
import numpy as np # 状态:d(距离),一维 def f(x, dt, u=0.0): # 距离随时间的缓慢漂移,u是环境修正项 return x + u * dt def H_jac(d): # 观测方程 v = K / d,导数 d(v)/d(d) = -K / d^2 return np.array([[-K / (d * d)]]) def predict(x, P, Q, dt, u=0.0): x_pred = f(x, dt, u) F = np.array([[1.0]]) # 线性状态转移 P_pred = F @ P @ F.T + Q return x_pred, P_pred def update(x_pred, P_pred, z, R): H = H_jac(x_pred[0, 0]) S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) # 观测残差:真实测量 - 预测测量 innov = z - K_CONST / x_pred[0, 0] x_new = x_pred + K @ innov P_new = (np.eye(1) - K @ H) @ P_pred return x_new, P_new这里 x 和 P 都用二维 numpy 数组而不是标量,是为了统一矩阵运算接口。predict 里 F 写死成 [[1.0]] 是因为状态转移本身线性;update 里 H_jac 在预测点取值。残差 innov 是实测电压 z 减去预测电压 K/d,这一步是 EKF 最容易写错的地方——一定要用 h(x_pred) 而不是直接用 x_pred 做差。
参数设置:dt 根据传感器采样周期定,比如 10Hz 取 0.1 秒;Q 取 0.01 表示我们对距离漂移模型的信任程度;R 取 0.05 对应电压噪声方差。你可以试着把 Q 调到 100,会看到滤波输出疯狂贴向测量值,抖动明显变大;再把 R 调到 100,输出变成一条迟滞的平滑曲线。这种“调 Q 看响应”的练习比看十遍公式都管用。
3.2 多维EKF:IMU+GPS松组合定位
现在把场景换成二维平面运动。状态取 x=(px, py, vx, vy, yaw, v),yaw 是航向角,v 是前向速度。控制输入是 IMU 给出的加速度 a 和转向角速度 w,观测是 GPS 输出的 (px, py)。状态转移是典型的非线性:先按航向把速度分解到 x、y 方向。
import numpy as np def f(x, u, dt): px, py, vx, vy, yaw, v = x.ravel() a, w = u # a 轴向加速度,w 转向角速度 yaw_new = yaw + w * dt v_new = v + a * dt px_new = px + v * np.cos(yaw) * dt py_new = py + v * np.sin(yaw) * dt # 注意:速度分解用的是当前航向 return np.array([[px_new, py_new, v * np.cos(yaw), v * np.sin(yaw), yaw_new, v_new]]).T def H_jac(x_pred): # 观测 h(x) = [px, py],所以 H 是 2×6 H = np.zeros((2, 6)) H[0, 0] = 1.0 H[1, 1] = 1.0 return H def F_jac(x, u, dt, eps=1e-6): # 6×6 状态转移雅可比,用数值差分代替手推 n = x.shape[0] F = np.zeros((n, n)) x0 = x.copy() for i in range(n): xp = x0.copy(); xp[i, 0] += eps xm = x0.copy(); xm[i, 0] -= eps fp = f(xp, u, dt) fm = f(xm, u, dt) F[:, i] = ((fp - fm) / (2 * eps)).ravel() return F这里面有个工程细节:vx、vy 是冗余状态,真正驱动位置更新的是 v 和 yaw,但观测里没有直接给 vx、vy,它们就只能靠协方差跨项间接修正。很多初学者在状态里同时放线速度和分速度,结果滤波发飘。我的建议是状态要么只放 (px, py, yaw, v),把 vx、vy 从状态里删掉;要么保留 vx、vy 但把状态转移明确写成 vx=vcos(yaw),vy=vsin(yaw),并接受模型误差。上面代码保留了 vx、vy 是为了展示这种结构的常见问题。
雅可比 F 这里用数值差分是因为手写解析式容易漏掉三角函数的链式法则。在实际调试中,我通常先用数值差分验证解析式,验证一次后长期用解析式,因为数值差分在高维状态里每一趟要算 2n 次 f,代价太高。你可以把上面 F_jac 里的注释解开,对比解析式和数值差分的结果,两者应该在小量级上一致。
3.3 把EKF封装成可复用类
我一般会把预测和更新拆成两个方法,状态模型和观测模型做成回调函数,这样换传感器只需要改观测回调。
class EKF: def __init__(self, fx, hjac, Fx=None, dt=0.1): self.fx = fx # 状态转移函数 self.hjac = hjac # 观测雅可比 self.Fx = Fx # 状态雅可比,None 时用数值差分 self.dt = dt def predict(self, x, P, u, Q): F = self.Fx(x, u, self.dt) if self.Fx else self._num_F(x, u) x_pred = self.fx(x, u, self.dt) P_pred = F @ P @ F.T + Q return x_pred, P_pred def update(self, x_pred, P_pred, z, R, hfun): H = self.hjac(x_pred) S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) innov = z - hfun(x_pred) x_new = x_pred + K @ innov P_new = (np.eye(len(x_new)) - K @ H) @ P_pred return x_new, P_new def _num_F(self, x, u): eps = 1e-6 n = x.shape[0] F = np.zeros((n, n)) for i in range(n): xp = x.copy(); xp[i, 0] += eps xm = x.copy(); xm[i, 0] -= eps F[:, i] = ((self.fx(xp, u, self.dt) - self.fx(xm, u, self.dt)) / (2 * eps)).ravel() return F这样封装的好处是:换模型只改传进来的函数,不碰滤波主流程。Q、R 可以做成属性,运行时动态调整。注意 np.linalg.inv 每次更新都做一次矩阵求逆,如果性能敏感,可以用 numpy.linalg.solve 替代:K = P_pred @ H.T @ np.linalg.inv(S) 改成先解 S 再乘。
3.4 仿真数据怎么喂给EKF
写完代码不能直接上真机,先造一段带真值运动轨迹的仿真数据。常见做法是:用某跨平台系统的运动学模型生成轨迹(圆轨迹或“8”字轨迹),叠加高斯噪声模拟 IMU 和 GPS,然后把带噪数据喂给滤波算法,与真值对比。代码一般长这样:
# 生成8字轨迹 t = np.linspace(0, 20, 2000) x_true = 10 * np.sin(0.3 * t) y_true = 5 * np.sin(0.6 * t) # 模拟GPS观测:每0.2秒一个,叠加方差1的高斯噪声 gps_ts = t[::20] gps_x = x_true[::20] + np.random.normal(0, 1, len(gps_ts)) gps_y = y_true[::20] + np.random.normal(0, 1, len(gps_ts)) # 模拟IMU:真值加速度+噪声 imu_ts = t imu_a = -10 * 0.3 * 0.3 * np.sin(0.3 * t) + np.random.normal(0, 0.1, len(t))跑完滤波后画误差曲线,观察误差是否收敛、有无跳变。这一步的价值在于,真机数据里你不知道真值,仿真里可以量化验证 EKF 的估计精度。注意仿真生成的数据要和 EKF 的 dt、观测时间戳严格对齐,否则误差曲线会出现虚假的周期性抖动,干扰判断。
4. EKF调参避坑:协方差发散、雅可比错误与单位混乱的五个血泪记录
4.1 现象:P矩阵对角线持续增长,状态几秒内飞出一个数量级
原因:Q 设置过小,模型不确定性被严重低估,滤波器把预测当成铁律,一旦遇到模型外误差,增益 K 趋向 0,观测再也拉不回来。我做某图像处理 Demo 的姿态解算时,把 Q 初始化为 0.001,静止状态下 P 对角线每隔几秒涨一个量级,状态直接漂到天上去。
解决:先把 Q 调到让滤波器在静止状态下也有明显修正,再逐步减小。诊断方法:打印每次更新的 K 矩阵,看它的对角线是否长期接近 0。如果 K 接近 0,说明滤波器认为预测比观测可靠得多,这时候十有八九是 Q 太小或 R 太大。
4.2 现象:P矩阵非正定,出现NaN
原因:P 更新用 (I−KH)P_pred,减法累积让对称性破坏;也可能是 H 矩阵维度写错,导致矩阵形状不匹配。某个项目里我把观测雅可比 H 写成 n×m,结果矩阵乘法直接抛异常,P 最后一行出现 NaN。
解决:改用 Joseph 形式 P_new=(I−KH)P_pred(I−KH)^T+KRK^T;每次更新后强制做对称化 P=(P+P.T)/2;并在 predict 之后检查 np.isfinite(P).all()。调试阶段加一条断言,一旦 P 异常立刻停住,别让坏数据继续污染后面的状态。
4.3 现象:状态在动,滤波输出却滞后一个节拍
原因:过程噪声 Q 太小,滤波器对预测的信任度超过对观测的信任度。也有人把原因归结为 R 太大,两者都会造成增益过小。现象是误差曲线明显滞后于真值曲线,看起来像传感器延迟。
解决:调大 Q 或调小 R。更稳妥的做法是利用残差序列的均值判断:残差长期同号,说明预测明显偏慢;把 Q 的加速度项加大 50% 再试。这个现象经常被误判为“传感器延迟”,其实把 Q 加大就好了。真实项目里,IMU 采样率高、GPS 采样率低,如果 GPS 数据自身带一个周期延迟,要先纠正时间戳,不要盲目调参。
4.4 现象:一阶线性化在强非线性下翻车,滤波发散
原因:EKF 在线性化误差大的前提下仍然假设高斯分布。比如观测方程是 atan2,接近 ±π 处跳变,H 无穷大,K 直接崩溃。我在处理雷达极坐标到直角坐标转换时遇到过,目标在正前方附近来回晃,角度残差在 π 和 -π 之间跳,滤波结果直接剧烈震荡。
解决:把这部分状态改用角度差值包装,让残差限定在 [−π,π] 之间;若还不行,换成 UKF 或 IEKF。IEKF 的做法是在更新步骤里迭代 2~3 次重算 H。角度归一化的典型写法是:
def wrap_angle(angle): return np.arctan2(np.sin(angle), np.cos(angle))4.5 现象:米、度、弧度混在一起,状态估计正常但协方差看起来奇怪
原因:不同物理量单位数值差异悬殊,比如位置是几千,角度是零点几弧度,导致 P 和 Q 对角元素跨好几个数量级。EKF 对矩阵条件数敏感,数值上病态,即使状态估计看起来正常,协方差也会失真,之后一旦遇到异常观测,滤波很容易崩溃。
解决:状态统一用国际单位,角度一律用弧度并手动做角度归一化;如果调参困难,可以先归一化后再进滤波器,输出时再逆变换。另一个务实策略是给 P 初始化为对角阵,对角元素按物理量真实量级设置,不要全填 1。
4.6 调试EKF前先装好的四个工具
我每次调 EKF 前会固定准备四件东西。第一,残差曲线图:横轴时间,纵轴 z−h(x̂_pred),正常情况应在 0 附近随机波动;如果残差有趋势性,说明模型或雅可比有问题。第二,P 矩阵的特征值曲线:特征值持续增长或变成负数,说明数值稳定性出问题。第三,K 矩阵的迹:迹过大说明滤波器容易被观测带跑。第四,仿真真值对比图:没有真值的调试都是盲人摸象,仿真数据是整个调参环节里的“后悔药”。
注意:上面四个工具里,残差图和 K 矩阵是最容易现场打印的,P 矩阵特征值适合离线分析。真机调试时不要同时打印多个矩阵,否则日志量大到你根本找不到关键信息。
5. 工程落地:EKF变体选择、嵌入式优化与一致性评估
5.1 什么时候该换UKF或粒子滤波
EKF 不是万能的,选型要看你系统的非线性强度和噪声分布。下面这张表是我做技术方案时的固定参考:
| 算法 | 原理 | 计算量 | 适用场景 |
|---|---|---|---|
| EKF | 一阶泰勒展开线性化 | 低 | 弱非线性、高斯噪声、算力受限 |
| UKF | 无迹变换,sigma点传播 | 中 | 强非线性、高斯噪声 |
| 粒子滤波 | 蒙特卡洛采样 | 高 | 强非线性、非高斯噪声 |
经验判断标准:如果 h(x) 或 f(x) 在正常工作范围内的高阶导数不可忽略,或者残差的统计特性明显偏离高斯,就优先考虑 UKF。比如视觉 SLAM 里的重投影误差,涉及相机内参、畸变和位姿,非线性强,很多方案直接跳 EKF 用 UKF。反过来,GPS+IMU 松组合这种弱非线性场景,EKF 完全够用,不必为了追赶潮流引入额外算力。
5.2 嵌入式平台的EKF优化
到了嵌入式平台,首先砍掉的是动态内存分配。n 维协方差矩阵哪怕 n=15,用 float32 存储也就 900 个字节,但要防止每次 predict/update 都 new 一个数组。常见做法是预先分配最大维度的静态数组,用宏控制实际维度。矩阵求逆尽量用 Cholesky 分解替代通用求逆,因为协方差矩阵是正定的,Cholesky 不仅快一倍,数值稳定性也好。
定点化是个大坑:EKF 的状态可能从 0.001 跨到 10000,float32 没问题,但如果硬件没有浮点单元,你要用 Q 格式定点数,就得特别注意 P 矩阵的量级范围。我一般会先在实际数据上跑一遍 float32 仿真,统计 P 对角元素的最小值和最大值,再选 Q 格式的整数位宽。角度的 Q 格式最容易溢出,别和位置共用同一套比例因子。
5.3 一致性评估:NEES与残差检验
EKF 调完之后,不能只看误差曲线贴着真值就结束,还要做一致性检验。NEES(归一化估计误差平方)的定义是:
NEES = (x_true − x̂)^T P^−1 (x_true − x̂)
对 Monte Carlo 运行取平均,应服从自由度为 n 的卡方分布。例如 n=6 时,95% 置信区间大约是 [6, 14] 附近;NEES 均值远大于上界,说明滤波器过于自信;远小于下界,说明 P 设得过大、过于保守。
from scipy.stats import chi2 # errors: (N, n) 每行是真实状态与估计状态之差 # Ps: (N, n, n) 每行是滤波输出的协方差 nees_avg = np.mean([ errors[i].reshape(1, -1) @ np.linalg.inv(Ps[i]) @ errors[i] for i in range(N) ]) # 自由度 n=6, 95% 区间 bounds = chi2.interval(0.95, df=6) print(f"NEES={nees_avg:.2f}, confidence interval: [{bounds[0]:.2f}, {bounds[1]:.2f}]")NEES 偏高时,优先检查 Q 是否太小;偏低时检查 R 是否太大。很多调参者只盯误差曲线,忽略了 NEES,结果换一个传感器噪声环境就发散。我在仿真阶段跑 20~50 次 Monte Carlo,把 NEES 稳在置信区间内,再上真机。
5.4 多传感器异步融合的常见做法
IMU 高频、GPS 低频、视觉 10Hz,这种异步输入在工程中极常见。处理思路有两种:第一种是预测高频执行,观测只在有测量时更新;第二种是多速率卡尔曼,为每个传感器维护独立的时间对齐缓冲。
# 伪代码:高频预测+低频更新 x, P = init() for step in range(total_steps): # 每个IMU tick都做预测 x, P = ekf.predict(x, P, imu_u, Q, dt_imu) # 当新的GPS观测到达,做一次更新 if gps_available: x, P = ekf.update(x, P, gps_z, R_gps, hfun_gps)注意时间戳处理:IMU 的 dt 固定是 1/imu_hz,但 GPS 到达时刻不一定和 IMU tick 对齐,需要把 GPS 时间戳和当前状态时间戳的差值算出来,用差值做一次额外的纯预测再更新,否则残差会引入系统偏差。这个坑我踩过:GPS 时间戳没对齐,残差序列始终有固定偏差,调了半天 Q 才发现根源。
6. 验证EKF实现是否正确的三个技巧:仿真注入、残差检查与卡方检验
先做仿真注入。在仿真轨迹中间加一个位置阶跃偏移,比如把 px_true 从第 100 帧开始直接加上 3 米,然后运行同一套 EKF。如果滤波器能在 1~2 秒内收敛回真值,说明状态转移和观测模型的方向是对的;如果收敛速度极慢或发散,先怀疑雅可比矩阵,再查 Q 和 R 的比值。仿真注入的妙处在于,你知道真实突变的时间和幅度,可以直接算收敛时间常数,这是真机做不到的。
接着看残差。残差定义为 e = z − h(x̂_pred),收集一段时间的残差序列,计算均值和标准差。均值应接近 0,标准差应接近 S = H P_pred H^T + R 的对角线开根。如果残差均值明显非零,说明存在系统偏差,可能是时间戳未对齐或传感器未标定;如果标准差远大于理论值,说明 R 设小了,滤波器把噪声当成了有效信号。检查残差不需要额外代码,滤波循环里打印一行日志就行。
最后跑卡方检验。运行 20 次 Monte Carlo 仿真,每次用不同的随机噪声种子,计算 NEES 并查看是否落在自由度 n 的 95% 置信区间内。这个检验对模型错误非常敏感:状态转移少算一项、观测方程漏了旋转矩阵,NEES 都会显著偏离区间。我某次在某跨平台系统里把旋转顺序搞反,误差曲线看着还行,NEES 却冲到 30 以上,靠这个检验才抓到问题。
从那以后,我每次写完 EKF 都强制先跑完仿真注入、残差检查、卡方检验这三步,再上真机,否则绝不上电。这三步跑完,至少能确定滤波器没有“黑匣子”式的隐性错误,剩下的只是参数微调。希望帮到你。
本文还有配套的精品资源,点击获取