简介面向非线性系统状态估计的UKF无迹卡尔曼滤波MATLAB代码适合正在学习滤波理论、开展目标跟踪、传感器融合或控制系统研究的学生与工程师。代码按UKF标准流程组织包含sigma点生成、预测更新、观测更新、滤波更新及迭代循环等核心函数并附带测试脚本借助这些模块可以理解无迹变换如何用采样点逼近非线性分布规避传统线性化误差同时可对照EKF和粒子滤波的估计结果了解各算法适用边界。压缩包共6个文件以4个m源程序为主另有2个asv自动备份文件整体仅3KB结构紧凑可直接放入MATLAB运行、调试并根据实际场景调整系统模型与噪声参数。已有5116人学习该资源通过研读和修改alpha、beta、kappa等无迹变换参数可进一步优化滤波精度并迁移到目标跟踪、多传感器融合等工程应用中。1. UKF无迹卡尔曼滤波算法Matlab代码非线性估计为什么绕不开无迹变换UKF无迹卡尔曼滤波算法的Matlab代码并不像网上说的那么玄学。一个做组合导航的开发者告诉我他把EKF换成UKF之后最直接的感受是不用手推Jacobian强非线性量测下滤波也不再动不动就飞掉。UKF的核心是用一组sigma点去传递状态的分布特征精度能到二阶而EKF只有一阶。这篇笔记不打算重复教科书推导直接围绕一份可运行的Matlab代码讲清楚sigma点怎么生成、时间更新和量测更新怎么写Q、R和alpha这些参数怎么设最后把协方差非正定、权重算错这类高频踩坑点列出来。适合正在用EKF但觉得精度不够的工程师也适合刚接触无迹变换的研究生照着代码跑一遍。2. UKF原理与选型无迹变换如何绕过Jacobian2.1 为什么不用EKF线性化误差与Jacobian的痛标准卡尔曼滤波只解决线性高斯问题进入非线性场景后最直接的想法是把当前状态附近的系统函数做一阶泰勒展开这就是EKF。EKF的问题出在展开点上如果系统函数的非线性较强比如量测方程里有平方根、反三角函数或者乘积项一阶截断误差会直接污染协方差更新。时间一长滤波器对自己的估计越来越自信协方差不断变小但真实误差却可能很大最终出现发散。我在给某模拟项目调EKF时量测方程是一个带开方的定位模型手推Jacobian花了整个下午还因为漏掉一个链式法则项导致滤波结果在30个周期之后彻底偏离。换成UKF之后状态转移和量测函数都变成黑匣子只要写清楚f(x)和h(x)就行不再需要求导。这不是说EKF完全不能用而是对于强非线性、难求导的场景UKF的性价比明显更高。尤其现在很多Matlab工程里系统模型本身就是函数句柄用UKF几乎不需要额外建模成本。UKF的另一个优势是对初始误差的容忍度更高。EKF在初始化不准时线性化点离真值很远泰勒展开的误差就更不可控UKF用sigma点在分布上采样即使初始协方差稍大也能把分布特征更完整地传到下一帧。这也是为什么组合导航、目标跟踪、无人车状态估计这类工程里UKF经常作为EKF的替代方案出现。2.2 sigma点生成的数学细节alpha、beta、kappa三个参数无迹变换的基本思想是与其把非线性函数在一点展开不如在均值附近取2n1个带权重的点让这些点经过非线性函数传播后再重新加权得到更新后的均值和协方差。n是状态维数所以sigma点数量是状态维数的两倍加一。对于二维定位问题n2共5个sigma点对于十维状态就是21个点计算量可控。构造sigma点前先要确定三个参数alpha、beta、kappa然后计算一个中间变量lambda alpha^2 * (n kappa) - n。alpha控制sigma点离均值的远近工程上常在1e-3到1之间取值alpha越小采样点越贴近中心对小尺度非线性更敏感但过大或过小都可能让权重出现明显负值。beta与先验分布形状有关如果认为状态服从高斯分布beta取2是经验值能在协方差重构时引入高阶项修正。kappa是一个辅助缩放参数通常取0也有实现取3-n目的是让更高阶矩在某些分布下更匹配。权重分两组均值权重Wm和协方差权重Wc。第一个sigma点的权重和其余点不同公式比较固定。我自己习惯把权重计算抽成独立函数避免在滤波主循环里重复性代码干扰逻辑。2.3 常见做法封装ukf_weights与ukf_predict让主循环保持整洁下面是一个最常用的Matlab权重生成函数可以直接抄进自己的脚本function [Wm, Wc, lambda] ukf_weights(n, alpha, beta, kappa) % 生成UKF的sigma点权重 % n: 状态维数 % alpha: 散布参数通常1e-3~1 % beta: 先验分布参数高斯分布取2 % kappa: 缩放参数通常取0或3-n lambda alpha^2 * (n kappa) - n; Wm zeros(2*n1, 1); Wc zeros(2*n1, 1); Wm(1) lambda / (n lambda); Wc(1) lambda / (n lambda) (1 - alpha^2 beta); for i 2:2*n1 Wm(i) 1 / (2 * (n lambda)); Wc(i) 1 / (2 * (n lambda)); end end逻辑说明Wm用于传播后对sigma点求加权均值Wc用于求加权协方差。两个权重只在第一个sigma点处不同这是无迹变换的标准结构。lambda必须最先算因为后续构造sigma点时需要用它来缩放协方差。参数说明第一点权重Wm(1)可能为负这是允许的只要所有Wm求和等于1即可beta只影响第一点的协方差权重不参与均值计算。时间更新预测部分同样适合封装。以下是ukf_predict函数function [x_pred, P_pred] ukf_predict(x, P, f, Q, Wm, Wc, lambda) % UKF时间更新 % x: 当前状态向量 % P: 当前协方差矩阵 % f: 状态转移函数句柄x_next f(x) % Q: 过程噪声协方差 n numel(x); sqrtP chol((n lambda) * P, lower); Xi zeros(n, 2*n1); Xi(:,1) x; for i 1:n Xi(:,i1) x sqrtP(:,i); Xi(:,in1) x - sqrtP(:,i); end Y zeros(n, 2*n1); for i 1:2*n1 Y(:,i) f(Xi(:,i)); end x_pred zeros(n, 1); for i 1:2*n1 x_pred x_pred Wm(i) * Y(:,i); end P_pred zeros(n, n); for i 1:2*n1 d Y(:,i) - x_pred; P_pred P_pred Wc(i) * (d * d); end P_pred P_pred Q; end逻辑说明先对缩放后的协方差做Cholesky分解得到下三角矩阵sqrtP再以当前状态为中心分别加减sqrtP的每一列构造出2n1个sigma点。所有sigma点经过状态转移函数f后成为Y随后用均值权重求预测状态用协方差权重求预测协方差最后叠加过程噪声Q。参数说明chol的lower选项得到下三角矩阵取它的列作为扰动方向如果某一步协方差不再正定chol会直接报错这个数值问题在避坑章节单独展开。3. 从零写一个最小可运行的UKF的Matlab代码用距离量测定位二维目标3.1 模型选型为什么用「两个参考点的距离量测」来验证UKF很多新接触UKF的人一上来就想跑几十维的完整系统结果代码里到处都是维度索引错误很难定位问题。常见做法是先选一个最小但足够非线性的例子把滤波核跑通再迁移到正式模型。二维定位是一个很好的测试床目标在平面上移动观测量是目标到两个固定参考点的距离。距离量测要开方显然是非线性而且状态只有二维画图、调参、看误差都很直观。状态方程先简化成随机游走模型也就是每个时刻在位置上叠加一个高斯扰动。这样状态转移函数退化成单位映射但量测方程仍然保持非线性足够验证UKF的核心逻辑。如果你想测试更复杂的运动模型后续把f替换成匀速或匀转弯模型即可滤波主循环完全不用动。3.2 主脚本生成真实轨迹、量测并调用自写的UKF滤波先写一个完整主脚本文件名可以叫ukf_demo.m% ukf_demo.m % 二维定位通过两个参考点的距离量测估计目标位置 rng(1); % 固定随机种子保证可复现 T 100; % 仿真时长 dt 1; A [0; 0]; % 参考点A坐标 B [100; 0]; % 参考点B坐标 % 真实轨迹随机游走 Q_true 0.25 * eye(2); % 真实过程噪声方差 x_true zeros(2, T); x_true(:,1) [50; 50]; for k 2:T x_true(:,k) x_true(:,k-1) sqrt(Q_true) * randn(2,1); end % 量测噪声 sigma_r 0.5; R sigma_r^2 * eye(2); % 生成距离量测 Z zeros(2, T); for k 1:T p x_true(:,k); Z(:,k) [norm(p - A); norm(p - B)] sigma_r * randn(2,1); end % 滤波初始值 x0 [30; 80]; P0 30 * eye(2); Q 0.25 * eye(2); % 滤波假设的过程噪声这里和真实一致 % 状态转移函数随机游走状态保持不变噪声由Q体现 f (x) x; % 量测函数到两个参考点的距离 h (x) [norm(x - A); norm(x - B)]; % 运行UKF [x_est, P_est] ukf_filter(x0, P0, f, h, Q, R, Z);逻辑说明这里真实轨迹用Q_true生成量测加独立高斯噪声。滤波器假设的过程噪声Q与真实一致属于“理想”实验适合验证代码本身是否有误。匿名函数h捕获了A和B这使得ukf_filter内部只需要调用h(x)而不必感知模型细节。参数说明P0不能设成0否则滤波器一开始就认为状态无误差后续量测修正能力会减弱Q0.25*eye(2)表示位置扰动的方差是0.25平方米和真值生成一致。主循环函数ukf_filter如下function [x_est, P_est] ukf_filter(x0, P0, f, h, Q, R, Z) % UKF滤波主循环 % x0: 初始状态 % P0: 初始协方差 % f: 状态转移函数句柄 % h: 量测函数句柄 % Q: 过程噪声协方差 % R: 量测噪声协方差 % Z: 量测序列维度为(量测维数 x 时间) n numel(x0); m size(Z, 1); T size(Z, 2); alpha 0.5; beta 2; kappa 0; [Wm, Wc, lambda] ukf_weights(n, alpha, beta, kappa); x_est zeros(n, T); P_est zeros(n, n, T); x x0; P P0; for k 1:T [x_pred, P_pred] ukf_predict(x, P, f, Q, Wm, Wc, lambda); [x, P] ukf_update(x_pred, P_pred, h, Z(:,k), R, Wm, Wc, lambda); x_est(:,k) x; P_est(:,:,k) P; end end逻辑说明主循环只有两个步骤先时间更新再量测更新。ukf_update负责用当前的量测值Z(:,k)修正预测状态。如果后面要改成带控制输入的模型只需要在f的函数签名里增加控制量ukf_filter的骨架不用变。参数说明alpha0.5、beta2、kappa0是我在这个二维例子里的默认组合权重会出现一个负值点但整体仍然满足归一化数值上稳定。量测更新函数ukf_update是UKF里最容易写错的部分function [x_upd, P_upd] ukf_update(x_pred, P_pred, h, z, R, Wm, Wc, lambda) % UKF量测更新 % x_pred: 预测状态 % P_pred: 预测协方差 % h: 量测函数句柄 % z: 当前量测 % R: 量测噪声协方差 n numel(x_pred); m numel(z); sqrtP chol((n lambda) * P_pred, lower); Xi zeros(n, 2*n1); Xi(:,1) x_pred; for i 1:n Xi(:,i1) x_pred sqrtP(:,i); Xi(:,in1) x_pred - sqrtP(:,i); end Z_sigma zeros(m, 2*n1); for i 1:2*n1 Z_sigma(:,i) h(Xi(:,i)); end z_pred zeros(m, 1); for i 1:2*n1 z_pred z_pred Wm(i) * Z_sigma(:,i); end S zeros(m, m); Pxz zeros(n, m); for i 1:2*n1 dz Z_sigma(:,i) - z_pred; dx Xi(:,i) - x_pred; S S Wc(i) * (dz * dz); Pxz Pxz Wc(i) * (dx * dz); end S S R; K Pxz / S; x_upd x_pred K * (z - z_pred); P_upd P_pred - K * S * K; % 数值保护协方差保持对称且正定 P_upd (P_upd P_upd) / 2; P_upd P_upd 1e-9 * eye(n); end逻辑说明量测更新前半段和预测类似把预测状态重新展开成sigma点再送入量测函数得到Z_sigma。随后计算量测预测均值z_pred、新息协方差S以及状态-量测互协方差Pxz。卡尔曼增益K Pxz / S最终修正状态和协方差。最后两行是数值保护能有效避免协方差在长周期运行后出现非对称或病态。参数说明Pxz / S用的是Matlab右除等效于Pxz * inv(S)但数值上更稳1e-9 * eye(n)是一个非常小的正则项只改变极小特征值不影响滤波精度。3.3 绘图与误差带怎么判断滤波是否收敛代码跑完后最重要的是判断滤波结果是否合理。先看轨迹曲线figure; plot(x_true(1,:), x_true(2,:), b-, LineWidth, 1.5); hold on; plot(x_est(1,:), x_est(2,:), r--, LineWidth, 1.5); legend(真实轨迹, UKF估计); xlabel(x / m); ylabel(y / m); axis equal; grid on;再看每一步的定位误差是否落在协方差对应的误差带内err sqrt(sum((x_est - x_true).^2, 1)); std_post zeros(1, T); for k 1:T P P_est(:,:,k); std_post(k) sqrt(P(1,1) P(2,2)); % 位置协方差的迹 end figure; plot(1:T, err, b-, LineWidth, 1.2); hold on; plot(1:T, 2 * std_post, r--, LineWidth, 1.2); plot(1:T, 3 * std_post, g--, LineWidth, 1.2); legend(估计误差, 2倍标准差, 3倍标准差); xlabel(时间); ylabel(误差 / m); grid on;逻辑说明如果滤波器设计合理真实误差应该大部分时间落在2倍标准差带内偶尔越过3倍标准差属于正常。如果误差长期超出3倍带说明协方差被低估了滤波器过度自信通常要调大Q或R。参数说明std_post用P(1,1)P(2,2)表示两个方向方差的叠加对应位置误差的平方期望因此与err比较时不需要再开方后乘2直接把误差和2*std_post比较即可。4. UKF参数调优与仿真评估Q、R和alpha这些参数到底怎么设4.1 Q和R影响什么先看量纲再调数量级滤波器的Q和R相当于你对模型和传感器的信任程度。Q太大滤波器认为模型本身很不准会更依赖量测导致估计噪声偏大Q太小滤波器过于信任模型量测修正太弱误差可能缓慢发散。R的影响正好相反R太大表示量测噪声大滤波器不敢跟随量测R太小时估计曲线会充满量测噪声的毛刺。调参时先看量纲。在我的定位例子里状态是位置单位是米过程噪声Q是位置方差单位是平方米量测是距离单位是米R是距离方差单位也是平方米。如果实际使用中发现轨迹抖动剧烈优先怀疑Q偏大或R偏小。可以分别将Q和R乘以10、除以10做对比试验不需要精调数量级对了滤波行为就会正常。注意如果把时变过程写成Q q * dt * eye(n)那么dt的单位也要和仿真步长一致。4.2 alpha、beta、kappa与lambda的经验取值alpha、beta、kappa这三个参数不是随便填的。alpha决定sigma点的散布范围如果设成1e-3这种极小值sigma点会紧贴着均值对强非线性函数反而可能丢失分布尾巴如果设成大于1sigma点离得太远又可能在局部非线性强的区域产生偏差。常见做法是取alpha0.5到alpha1之间再配合beta2和kappa0能在多数工程场景下获得稳定结果。下面是我常用的几个组合参考可以按模型非线性程度选择alphabetakappa适用场景1e-320接近高斯的弱非线性追求高阶精度0.520中等非线性数值稳定性较好123-n状态维数较高时避免负权重过大0.500不想对分布形状做假设的通用选择lambda是在代码中直接参与sigma点缩放的量实际不需要手动传入由ukf_weights根据alpha、kappa、n自动计算。如果代码报出“矩阵接近奇异”之类的错误可以先检查n lambda是否接近0。当alpha非常小、n又大时lambda会非常接近-nnlambda接近0Cholesky分解就会出问题。这时把alpha改成0.5或者1问题通常会消失。4.3 用RMSE和NEES验证滤波质量调参不能靠眼睛看曲线至少要用指标量化。最常用的是RMSE也就是均方根误差。可以写一个脚本把真值和估计值逐点求误差rmse sqrt(mean(sum((x_est - x_true).^2, 1)));这个指标综合了x和y两个方向的误差。如果只关心某个方向可以用sqrt(mean((x_est(1,:) - x_true(1,:)).^2))。RMSE越小说明估计越准但它不能反映协方差是否可信。一个更严格的一致性指标是NEES归一化估计误差平方nees zeros(1, T); for k 1:T dx x_est(:,k) - x_true(:,k); P P_est(:,:,k); nees(k) dx / P * dx; end n_avg mean(nees) / n;逻辑说明NEES的理想均值接近1。如果n_avg远大于1说明滤波器输出的协方差偏小估计误差比滤波器认为的大很多如果远小于1说明协方差被高估滤波器过于保守。参数说明dx / P * dx在Matlab中计算的是马氏距离平方注意不要直接写成inv(P)数值上容易引入额外误差。调参时我一般先固定R调Q让NEES均值落在1附近再微调R看RMSE是否进一步下降。如果Q和R都合理但NEES仍然偏大问题就可能出在初始化P0或者模型本身而不在滤波器代码里。5. UKF在Matlab实现中的避坑记录从协方差非正定到sigma点权重错乱5.1 现象chol报错提示矩阵必须是正定矩阵运行UKF时第一个常见错误就是chol((n lambda) * P, lower)报错说矩阵不是正定矩阵。原因通常是协方差P在迭代过程中失去了正定性或者是alpha取太小导致nlambda接近0导致缩放后的矩阵在半精度附近变得病态。解决办法分两步。第一步在chol之前对P做对称化处理并加上一个很小的正则项P (P P) / 2; P P 1e-9 * eye(n);第二步检查n lambda的数值是否过小。我一般把alpha从1e-3改成0.5或1通过调整ukf_weights传入的参数来规避。如果问题出现在状态转移之后通常是过程噪声Q给的太小导致预测协方差退化此时适当增大Q也是一个有效方向。5.2 现象sigma点权重之和不为1滤波结果严重偏离有的实现里用户手动修改权重公式结果Wm求和不是1。典型错误是把Wm(1)和Wc(1)的公式记混或者循环从1开始写导致第一个点的权重被重复覆盖。权重求和不等于1的后果是加权后的预测均值带有系统性偏移滤波器会出现一个方向性的偏差。解决方法是写一个自检脚本[Wm, Wc, lambda] ukf_weights(2, 0.5, 2, 0); % 手工验证 sum(Wm) % 应该等于1 sum(Wc) % 应该等于1如果sum(Wc)不为1检查第一项Wc(1)是否漏加了(1 - alpha^2 beta)。这个修正项是UKF区别于朴素sigma点采样的关键千万不能省略。如果只想快速验证也可以在所有sigma点权重都相等alpha1kappa3-nbeta0时运行此时Wm和Wc都是1/(2n1)最容易检查。5.3 现象状态维数n变成0或1导致sigma点数量不对在封装函数时如果习惯用length(x)而不是numel(x)某些场景下可能意外拿到状态向量的其中一个维度。尤其是当x是行向量时length返回1但实际状态维数可能是2或更高。这会导致sigma点数量变成3而不是5滤波结果自然不对。解决方法是统一用numel(x)计算状态维数并在主脚本初始化时保证状态是列向量。还有一个隐蔽问题量测维数m如果取错Z_sigma矩阵的维度会和量测函数输出不匹配。我通常会在ukf_update里加一行断言assert(isequal(size(Z_sigma), [m, 2*n1]), 量测维数与量测函数输出不匹配);这样一旦出错Matlab会直接停在问题行不用回头猜。5.4 现象量测更新后协方差矩阵不对称误差带忽大忽小协方差不对称通常来自数值计算截断误差。P_upd P_pred - K * S * K在理论上是实对称矩阵但计算机浮点运算会让非对角元素出现微小偏差。该偏差经过多次迭代被放大最终会导致chol报错或者误差带上下不对称。解决手段是在每次量测更新末尾强制对称化P_upd (P_upd P_upd) / 2;这行代码不是多余保护而是在长周期仿真里的必要操作。也可以直接用状态误差方差的平方根来画误差带但根本问题还是要保证协方差合法。如果对称化后仍然出现负特征值可以进一步加正则项。5.5 现象滤波器发散误差只增不减误差发散的表现是估计曲线逐渐远离真值同时协方差可能仍然很小。这个现象很多时候不是代码bug而是模型失配。最常见的失配是滤波器过程噪声Q远小于真实扰动导致滤波器认为状态变化很小量测修正又被过低的协方差削弱最后滤波跟不上目标。解决方法是先检查新息序列也叫残差序列innov z - z_pred;如果新息均值明显不为0或者新息方差和S的理论值相差很大说明模型或参数有问题。我通常会先调大Q增大滤波器对状态不确定性的估计再观察NEES指标是否回到1附近。如果Q已经很大仍然发散就要检查量测函数是否写错尤其是坐标原点和参考点取错导致量测一直有常数偏置。6. 值得一试的进阶技巧用蒙特卡洛和函数句柄把UKF变成通用模块单次仿真通过只能说明代码能跑不能说明滤波算法稳不稳定。我现在的习惯是任何UKF代码都要跑一遍蒙特卡洛验证。也就是随机生成几十组初始误差、量测噪声重复运行同一套滤波器最后统计RMSE均值和NEES均值。这样能把偶然一次收敛良好的假象过滤掉也能暴露出参数边界。下面是一个极简的蒙特卡洛框架Nmc 50; rmse_all zeros(Nmc, 1); nees_all zeros(Nmc, 1); for mc 1:Nmc rng(mc); % 重新生成真值、量测、初始状态 % ... 和主脚本相同的代码 ... [x_est, P_est] ukf_filter(x0, P0, f, h, Q, R, Z); rmse_all(mc) sqrt(mean(sum((x_est - x_true).^2, 1))); % 计算NEES均值 end fprintf(RMSE均值: %.4f, NEES均值: %.4f\n, mean(rmse_all), mean(nees_all));逻辑说明每次循环都重置随机种子让不同次实验之间做到可控复现。如果NEES均值稳定在0.8到1.2之间说明滤波器的协方差输出是可信的如果RMSE均值虽然小但NEES远大于1则协方差不可信后续做数据融合时会出问题。另一个提升复用性的技巧是把状态转移函数和量测函数都做成函数句柄。这样同一套ukf_filter代码可以用于无人机定位、车辆导航、目标跟踪等不同项目只需替换f和h滤波核心完全不动。我在项目里通常还会把f和h定义在单独的脚本里方便调试。最后分享一个习惯UKF不是万能药它和EKF一样依赖模型精度只是对非线性的处理更优雅。真正把滤波跑飞的时候我的第一反应不再是翻滤波代码而是先画新息序列确认量测函数和噪声参数是否合理。记住滤波器只是个载体模型可靠UKF才能给你可靠。希望帮到你。本文还有配套的精品资源点击获取