简介面向非线性系统状态估计任务压缩包内提供基于无迹卡尔曼滤波UKF的MATLAB实现。UKF通过无迹变换生成一组sigma点经非线性函数映射后近似新的均值与协方差相比传统扩展卡尔曼滤波避免了对模型线性化的依赖更适合处理强非线性动态系统。压缩包仅含1个m文件大小约2KB代码精简便于直接阅读、修改和迁移到导航、控制、信号处理等实际场景。文件通常涵盖系统状态转移函数、观测函数、过程噪声与观测噪声协方差设置以及初始状态参数定义并完整执行预测和更新两个阶段的迭代滤波最终输出状态估计序列或滤波误差在此基础上读者可对照代码逐行理解sigma点生成、权重计算、卡尔曼增益更新等关键细节或作为基础模板替换自身状态方程与观测方程。已有345人学习浏览适合需要快速上手UKF原理或为具体非线性系统编写滤波算法的MATLAB用户参考。1. ukf.zip 里的无迹卡尔曼滤波解决的是非线性系统状态估计的最后一公里做非线性系统状态估计的人迟早会碰到同一个选择题运动方程、传感器量测和仿真数据都在但标准卡尔曼滤波的线性假设撑不住了。扩展卡尔曼滤波在弱非线性下还能凑合方程一旦出现平方项或饱和特性一阶泰勒展开的截断误差就会让协方差失真滤波要么慢半拍要么直接发散。无迹卡尔曼滤波UKF不对方程做线性化而是用一组 sigma 点直接穿过非线性函数再按统计量还原均值与协方差对高斯分布的非线性映射精度达到三阶计算量只比 EKF 多一个量级。ukf.zip 对应的正是这套非线性系统状态估计实现路径。下文从无迹变换讲起给出可改用的 Python 代码再落到噪声调优与一致性验证适合刚接手滤波项目的工程师。2. 无迹变换UKF 逼近非线性系统的核心sigma 点设计与参数含义2.1 为什么 EKF 在强非线性下会失准EKF 的核心操作是把非线性函数在估计点处做一阶泰勒展开然后沿用线性卡尔曼的预测—更新框架。这个做法的隐含前提是函数在展开点附近足够“直”当状态方程或量测方程的非线性程度不高时Jacobian 矩阵能较好地描述局部斜率误差可以被接受。问题出在强非线性场景。以 y x²/20 这类量测方程为例当真实状态在零点附近摆动时一阶近似把均值映射到 0而真实映射后的均值应该是正数同时协方差经过 Jacobian 缩放后与真实散布相差很远。均值偏差不断累积最终表现为滤波发散。很多现场排查到最后发现不是噪声设错而是线性化本身引入了系统性偏差。UKF 换了一条路不对方程做近似而是对概率分布做近似。它选取一组确定的采样点sigma 点让这些点的样本均值和协方差与原分布完全一致然后把每个点都送入非线性函数再对输出做加权统计。因为每个点都经过了真实的非线性映射均值和高阶矩的信息被保留下来这就是无迹变换Unscented Transform的含义。2.2 sigma 点生成与权重最常用的是 Merwe 标度对称采样对 n 维状态取 2n1 个点。先算标度参数lambda α²(n κ) - n然后生成 sigma 点X₀ x Xᵢ x (√((n λ)P))ᵢ Xₙ₊ᵢ x - (√((n λ)P))ᵢ其中 √((nλ)P) 表示对矩阵 (nλ)P 做 Cholesky 分解得到的下三角矩阵下标 i 表示取第 i 行向量。均值与协方差权重分别为Wm₀ λ/(nλ) Wc₀ λ/(nλ) (1 - α² β) Wmᵢ Wcᵢ 1/(2(nλ))采样点围绕状态均值对称分布散布范围由 α 和 P 共同决定。P 必须是半正定矩阵否则 Cholesky 分解直接报错这是 UKF 实现里最常见的崩溃点。2.2.1 alpha、beta、kappa 三个参数的物理含义α 控制 sigma 点与均值的距离取值范围通常在 1e-4 到 1 之间。α 越小采样点越靠近均值对局部非线性越敏感但过小时采样点几乎重合数值上会失去对高阶项的捕获能力。β 用于补偿先验分布的非高斯程度对高斯分布取 2 最优分布重尾时可适当加大。κ 是二次标度修正默认取 0状态维数 n 较大时常用经验值 3 - n让采样点的总体散布更贴近原分布。这三个参数不像 Q、R 那样对应物理噪声它们改变的是采样策略而不是模型。调参时先固定 α1e-3、β2、κ0 的三件套除非遇到数值问题否则不必先动它们。2.3 无迹变换的 Python 最小实现用 numpy 实现 Merwe 采样只需要十几行。下面这个函数生成 sigma 点与对应权重后面预测和更新两步共用import numpy as np def merwe_sigma_points(x, P, alpha1e-3, beta2.0, kappa0.0): n x.shape[0] lam alpha**2 * (n kappa) - n # 协方差必须半正定先做对称化防止 Cholesky 失败 P (P P.T) / 2.0 sqrtP np.linalg.cholesky((n lam) * P) Wm np.full(2 * n 1, 0.5 / (n lam)) Wc np.full(2 * n 1, 0.5 / (n lam)) Wm[0] lam / (n lam) Wc[0] lam / (n lam) (1 - alpha**2 beta) X np.zeros((2 * n 1, n)) X[0] x for i in range(n): X[i 1] x sqrtP[i] X[n 1 i] x - sqrtP[i] return X, Wm, Wcnumpy 的 cholesky 返回下三角矩阵直接取行向量做加减即可不需要转置。权重数组的长度与 sigma 点一一对应后续加权求和全部用向量化写法。lambda 的默认值刻意避开lambda关键词与np.linalg的命名冲突实际项目里按需改成可配置参数即可。验证变换是否正确有一个简单办法把非线性函数设成恒等映射变换后的均值应当严格等于输入 x协方差应当严格等于 P。如果这一步对不上问题一定出在权重或 Cholesky 用法上而不是后续滤波逻辑。3. 用 Python 实现 UKF 状态估计预测-更新循环的完整代码3.1 状态方程与量测方程的离散形式UKF 的递推结构与卡尔曼滤波同构只是把“线性矩阵运算”换成“sigma 点穿过非线性函数”。考虑一个经典的标量非线性基准系统x₍ₖ₊₁₎ 0.5xₖ 25xₖ/(1 xₖ²) 8cos(1.2k) wₖ zₖ xₖ²/20 vₖ第一个方程同时包含线性项、非线性项和时变驱动项第二个方程是平方量测。这个模型常被用来对比 EKF 与 UKFx 接近 0 时状态方程的一阶导数几乎为零EKF 的 Jacobian 会严重低估不确定性。仿真中设 wₖ ~ N(0,1)、vₖ ~ N(0,1)下面用两个函数描述 f 和 h使预测与更新步骤复用同一套 sigma 点逻辑。3.2 UKF 预测步sigma 点穿过状态方程def ukf_predict(f, x, P, Q, alpha1e-3, beta2.0, kappa0.0): n x.shape[0] X, Wm, Wc merwe_sigma_points(x, P, alpha, beta, kappa) # 每个 sigma 点独立穿过非线性状态方程 Y np.array([f(X[i]) for i in range(2 * n 1)]) # 加权还原预测均值与协方差 x_pred np.sum(Wm[:, None] * Y, axis0) P_pred Q.copy() for i in range(2 * n 1): d Y[i] - x_pred P_pred Wc[i] * np.outer(d, d) return x_pred, P_predP_pred 先拷贝 Q 再累加保证过程噪声进入协方差预测用Q.copy()而不是直接引用 Q避免污染外部变量。加权求和时Wm[:, None]把权重扩成列向量与 Y 逐行相乘后沿行方向求和等价于对所有 sigma 点输出做加权平均。参数 Q 的维度必须与状态一致标量系统传 shape 为 (1,1) 的数组不要传 Python 标量。3.3 更新步量测空间的协方差与卡尔曼增益def ukf_update(h, x_pred, P_pred, z, R, alpha1e-3, beta2.0, kappa0.0): n x_pred.shape[0] X, Wm, Wc merwe_sigma_points(x_pred, P_pred, alpha, beta, kappa) # sigma 点穿过量测方程得到量测预测分布 Z np.array([h(X[i]) for i in range(2 * n 1)]) z_pred np.sum(Wm[:, None] * Z, axis0) # 量测协方差 Pzz 与新息互协方差 Pxz Pzz R.copy() for i in range(2 * n 1): d Z[i] - z_pred Pzz Wc[i] * np.outer(d, d) Pxz np.zeros((n, Z.shape[1])) for i in range(2 * n 1): dx X[i] - x_pred dz Z[i] - z_pred Pxz Wc[i] * np.outer(dx, dz) K Pxz np.linalg.inv(Pzz) x_new x_pred K (z - z_pred) P_new P_pred - K Pzz K.T return x_new, P_new更新步在量测空间再做一次无迹变换用预测均值与协方差重新采样 sigma 点过量测方程得到量测预测 z_pred、量测协方差 Pzz 和互协方差 Pxz。卡尔曼增益 K 由 Pxz 乘 Pzz 的逆得到形式上与线性卡尔曼完全一致但所有统计量都由采样点还原。P_new 用P_pred - K Pzz K.T在 K 为最优增益时与 Joseph 形式等价如果后续改成次优增益建议换用 Joseph 形式保证对称正定。3.4 跑通一次完整仿真真值、量测与估计结果np.random.seed(42) T 100 def f_next(x, k): return np.array([0.5 * x[0] 25 * x[0] / (1 x[0]**2) 8 * np.cos(1.2 * k)]) def h_meas(x): return np.array([x[0]**2 / 20.0]) x_true np.zeros(T 1) z_store np.zeros(T) x_est np.zeros(T 1) P np.array([[1.0]]) Q np.array([[1.0]]) R np.array([[1.0]]) x_true[0] 0.1 x_est[0] 0.1 for k in range(1, T 1): # 仿真过程状态推进 量测生成 x_true[k] f_next(x_true[k-1], k-1)[0] np.random.normal(0, 1) z_store[k-1] h_meas(x_true[k])[0] np.random.normal(0, 1) # 滤波过程预测 更新 x_pred, P_pred ukf_predict(lambda x: f_next(x, k-1), x_est[k-1], P, Q) x_est[k], P ukf_update(h_meas, x_pred, P_pred, z_store[k-1], R) rmse np.sqrt(np.mean((x_true[1:] - x_est[1:])**2)) print(fUKF RMSE: {rmse:.4f})lambda x: f_next(x, k-1)把时间变量 k 绑定进状态方程让预测函数只接受状态向量保证 ukf_predict 的签名保持简单。lambda 对循环变量的捕获是引用式的但因为每个 k 迭代内立即调用不会出现晚绑定问题。跑完 T100 步后 RMSE 通常落在 0.6 到 1.0 之间具体值随随机种子浮动。想对比 EKF把同一组仿真数据喂给带 Jacobian 的版本观察强非线性段的峰值误差差距即可。4. UKF 工程落地的参数调优噪声矩阵、alpha 系数与发散判断4.1 Q 和 R 怎么定量纲、标定与比例关系R 可以从传感器离线数据直接估计采集一段静止或匀速段量测减去参考真值后的样本方差就是合理的 R 初值。Q 更难从数据直接拿到常见做法是先按状态方程驱动项的方差量级给初值再作为调参旋钮调整 Q 与 R 的比例。真正起作用的是 Q 与 R 的相对比例比例不变时整体放大缩小只影响协方差的绝对值不改变增益。参数物理含义推荐初值来源调大后果调小后果Q过程噪声协方差状态方程未建模项方差增益增大跟踪快但抖动响应迟缓滞后加大R量测噪声协方差传感器标定残差方差曲线平滑量测信任降低易震荡离群点被放大αsigma 点散布系数固定 1e-3采样点远离均值采样点贴近均值β先验分布峰度补偿2.0高斯最优适配重尾分布峰度更尖锐κ高阶标度修正0 或 3-n采样点总体距离变化同上反向工程上最隐蔽的坑是单位失配。状态量是弧度、量测量是角度时R 按角度标定会导致协方差数值差出约 57 倍滤波表现就是持续震荡。排查发散问题前先统一单位再看比例。4.2 alpha 引起的数值退化sigma 点贴均值α 取 1e-3 是 Merwe 论文的标准值但对高维或强非线性系统这个值会让 sigma 点离均值太近量化误差反而变大。判断方法是在滤波循环里打印 sigma 点与均值的距离如果量级小于 1e-6说明采样点已经重合数值上退化成单点递推此时 UKF 和直接跑状态方程没有区别。处理方式有两种把 α 调到 1e-2 或 1e-1或者把 κ 设为 3 - n。修改后要同步检查 Wc₀ 的符号α 变大时 (1 - α² β) 可能让零号权重变负协方差累加里出现负权重是正常现象不需要修正但要注意 (n λ) 不能接近零或变负否则 Cholesky 分解会失败。4.3 滤波器发散的三个早期信号与处理发散通常不是瞬间发生的以下三个信号按出现频率排序新息序列持续同号且远超量测噪声标准差。正常情况下 z - z_pred 在零附近正负交替连续 10 步以上同号多半是模型偏差或 Q 偏小。协方差矩阵出现负对角元或明显不对称。每步更新后做(P P.T) / 2对称化Python 里可检查np.linalg.eigvalsh(P)的最小特征值是否小于 -1e-10。状态估计出现跳变。这通常是 R 偏小导致增益过大量测离群点被当成真实状态。工程上可以在更新步之前加一个新息门限把超出门限的量测直接丢弃或降权处理innov z - z_pred innov_cov Pzz threshold 3.0 * np.sqrt(np.diag(innov_cov)) if np.abs(innov) threshold: # 量测可能为野值跳过更新步保持预测结果 x_est[k], P x_pred, P_pred else: x_est[k], P ukf_update(h_meas, x_pred, P_pred, z_store[k-1], R)门限取 3 倍量测协方差标准差对应高斯分布下约 99.7% 的置信区间。这个逻辑放在 ukf_update 外部便于单独统计野值剔除率。处理顺序建议是先确认量测单位与状态单位一致再调 Q/R 比例最后才动 α。大多数发散问题在第一步就能定位。5. 用 NEES 检验验证 UKF 的一致性无真值时的调参依据NEESNormalized Estimation Error Squared归一化估计误差平方回答的问题是滤波器自报的协方差到底可不可信。定义为εₖ (x_true,k - x̂ₖ)ᵀ Pₖ⁻¹ (x_true,k - x̂ₖ)对 n 维状态做 M 次蒙特卡洛仿真每次使用相同真值和噪声统计、不同随机种子得到每个时刻的平均 NEES。M 次独立试验累加后M·εₖ 服从自由度为 M·n 的卡方分布95% 置信区间为 [χ²(0.025, M·n)/M, χ²(0.975, M·n)/M]。平均 NEES 持续高于上界说明滤波器过度自信自报协方差偏小应增大 Q 或减小 R持续低于下界则说明协方差偏大滤波器保守可减小 Q。调参顺序固定先锁定 α/β/κ用 NEES 把 Q/R 比例调对再微调 α 改善峰值误差。代码骨架如下from scipy.stats import chi2 M, n_states, T 50, 1, 100 low chi2.ppf(0.025, M * n_states) / M high chi2.ppf(0.975, M * n_states) / M nees_sum np.zeros(T) for run in range(M): np.random.seed(run) # 滤波循环内保存每个时刻的 error 与 P此处省略相同部分 err x_true[1:] - x_est[1:] nees err**2 / P_saved nees_sum nees nees_avg nees_sum / M ratio np.mean((nees_avg low) (nees_avg high)) print(f一致性通过率: {ratio * 100:.1f}%)注意蒙特卡洛次数 M 不能太小M50 时卡方上下界约在 0.62 到 1.42 之间区分度足够。现场没有真值的情况下把 NEES 中的误差换成新息就得到 NISNormalized Innovation Squared量测 z 本身可在线获得同一套卡方区间判断可以直接搬进监控程序作为 UKF 长期运行的健康指标。这一步检验做完UKF 的协方差输出才有实际参考价值后续做置信区间、故障检测或传感器融合才有依据。本文还有配套的精品资源点击获取