简介：针对非线性系统状态估计难题，这套UKF无迹卡尔曼滤波MATLAB代码给出了清晰简洁的实现，适合正在学习现代滤波理论、需要快速搭建并调试UKF模型的学生和科研人员。压缩包内共6个文件，包含4个.m源码（如核心滤波ukf.m、sigma点生成sigmas.m、无迹变换ut.m及测试脚本test.m）和2个.asv自动备份文件；整体仅3KB，结构紧凑，方便逐行理解算法细节。已有5108人浏览学习，被广泛用作入门UKF的参考代码。借助代码中完整的初始化、无迹变换、预测与观测更新流程，读者可以掌握协方差传播和卡尔曼增益的计算要领；通过调整噪声参数、无迹变换系数和系统模型，还可将算法迁移到目标跟踪、传感器融合等实际项目中，进一步对比EKF或粒子滤波的精度差异。 做状态估计的人，迟早会撞上“非线性”这堵墙。卡尔曼滤波在航空航天、目标跟踪、电池SOC估计这些场景里确实很能打，但它从骨子里是线性高斯假设的产物。一旦状态方程或量测方程里出现平方、三角函数、指数这类非线性项，标准KF直接失效。这时候大部分人第一反应是扩展卡尔曼滤波（EKF），把模型在估计点附近做一阶泰勒展开，好是好，可碰到强非线性，线性化误差会让滤波器发散给你看。我换到无迹卡尔曼滤波（UKF）之后，情况才真正好转——它不需要算雅可比矩阵，直接用一组Sigma点去“穿透”非线性函数，精度能到泰勒展开的二阶甚至更高阶，代码量却只比KF多了一截。这篇博客我就把UKF的核心原理和一份能直接跑起来的Matlab实现拆开讲清楚，适合刚入门滤波算法、或者已经在用EKF但被非线性折腾得头疼的朋友参考。
1. 为什么不用卡尔曼滤波而选UKF？先从线性假设说起
1.1 标准KF的适用边界
标准卡尔曼滤波的本质是贝叶斯框架下的最优线性估计。它假设系统的状态转移和量测过程都是线性的，噪声是零均值高斯白噪声。在这个前提下，KF给出的是均方误差意义下的最优解，而且计算量小、实现简单，所以工程上用得极其广泛。
但现实世界几乎不存在纯粹的线性系统。拿锂电池SOC估计来说，开路电压（OCV）和荷电状态（SOC）之间是明显的非线性曲线；拿雷达目标跟踪来说，量测通常是在极坐标系下得到的距离和角度，而状态却是在直角坐标系下描述的，中间隔着平方和反正切运算。只要模型里出现任何非线性项，KF的那套“协方差一步预测+增益更新”就开始不成立了——因为高斯分布经过非线性变换之后不再服从高斯分布，你再拿线性高斯那套公式硬套，结果自然是偏的。
1.2 非线性问题的三条路：EKF、UKF、粒子滤波
对付非线性，业界主流有三条路。第一条是EKF，思路最简单：在当前估计点附近对非线性函数做一阶泰勒展开，把问题重新“掰回”线性框架里跑标准KF流程。优点是实现容易、计算量小；缺点也很明显，一阶近似在强非线性场景下会丢掉高阶项信息，而且雅可比矩阵的推导容易出错，遇到不可导函数直接傻眼。
第三条是粒子滤波，它对状态分布不做任何高斯假设，用一堆随机粒子去逼近任意分布，非线性再强也不怕。但代价是计算量巨大，而且存在粒子退化和样本贫化问题，工程实时性要求高的场合很难用。
UKF夹在两者中间，走的是第三条路。它不做函数线性化，而是用“无迹变换”挑选一组确定性样本点（Sigma点），让这些点去经过非线性函数，再用变换后的点还原出均值与协方差。这条路既绕开了雅可比矩阵，又不需要成千上万个粒子，精度通常能达到二阶，计算量只比EKF高一个量级不到，性价比非常高。
1.3 UKF核心思路：不是近似函数，而是近似分布
EKF和UKF的根本分歧在于近似对象。EKF近似的是非线性函数本身，它把函数曲线拉直成切线，误差大小完全取决于当前点的曲率；UKF近似的是状态分布，它用多个Sigma点去“采样”分布，然后让每个点独立通过非线性函数，最后加权统计得到输出分布。本质上UKF是在做“对分布的蒙特卡洛式传播”，只不过用的不是随机样本而是精心设计的确定性样本。
形象一点说：EKF像是一个近视眼选手，隔着很远的距离去看曲线，只看得到它的大致走向，细节全糊了；UKF则像派出一支侦察小队，每个人站在不同的位置上，各自报回自己看到的局部信息，最后汇总出整条曲线的形状。“分布近似”的思路让UKF在强非线性场景下天然比EKF稳。
2. 无迹变换到底在做什么？Sigma点与权重解析
2.1 从“确定性采样”理解无迹变换
无迹变换（Unscented Transform，UT）是整个UKF的地基。它的目标很明确：已知一个随机变量x的均值x̄和协方差P，经过非线性函数y=f(x)之后，求y的均值和协方差。
做法分三步。第一步，构造2n+1个Sigma点（n为状态维度）：
其中(i=1,...,n)，表示矩阵平方根的第列，可以通过Cholesky分解得到。第二步，把每个Sigma点代入非线性函数得到。第三步，按权重加权合成y的均值与协方差：
这里的权重不是随便给的，它们要保证重构出的均值与协方差能够匹配原始分布的矩信息。无迹变换这个“无迹”的说法，指的就是这套采样-传播-加权的过程能够精确捕获高斯分布的前二阶矩。
2.2 三个参数alpha、beta、kappa怎么调
UT里有三个可调参数，调法直接决定滤波表现。
alpha决定Sigma点在均值周围的散布范围，取值通常在到之间。alpha越接近0，Sigma点离均值越近，对强非线性的局部分辨能力越强，但过小时可能引入数值计算问题；alpha取1时Sigma点离均值最远，稳定性好但精度会打折。我一般从开始试，跑通后再根据结果微调。
beta是与先验分布相关的参数，高斯分布时取2是最优的，这一点有严格的数学推导支撑。如果你能确定噪声不是高斯分布，再考虑改这个值。
kappa是次级缩放参数，一般取0即可，也可以取来匹配分布的高阶矩。需要注意当kappa取负值时，第一个权重可能变成负数，协方差矩阵的正定性会受影响，新手不建议动这个参数。
回头看我给出的代码，lambda=alpha^2*(n+kappa)-n这个式子很重要，它把三个参数整合在一起，直接影响Cholesky分解能否成功——如果算出来是个负数，chol函数会直接报错。
2.3 维度灾难？UKF的计算量其实很友好
很多人一听“采样”两个字，就担心粒子滤波那种大计算量。其实UKF的Sigma点数量只有2n+1个，对于常见的低维状态空间（n=2到10），也就是5到21个点。每个点做一次状态传播和量测传播，即使加上协方差合成的矩阵运算，总计算量仍然很少，在Matlab里跑上千步仿真也就是毫秒级的事。
我做过一个三维姿态估计的例子，状态维度9，UKF每步的计算耗时大约是EKF的1.5到2倍，但得到的精度提升明显，这个代价值得付。相比动辄几千个粒子的粒子滤波，UKF的计算量可以说不值一提。
3. Matlab代码逐步拆解：一个能直接跑的非线性估计例子
3.1 仿真场景与系统模型
为了把UKF讲透，我选一个经典的非线性一维系统，这个模型很多滤波论文里都出现过：
- 状态方程：
- 观测方程：
其中过程噪声和量测噪声，方差分别是和。这个系统的状态方程里有分数项和余弦项，观测方程里有平方项，非线性程度比纯粹的线性模型强不少，很适合用来对比EKF和UKF。我跑100步仿真。
3.2 完整代码：参数初始化与Sigma点生成
clear; clc; close all; % ===== 系统模型 ===== % x(k+1) = 0.5*x(k) + 2.5*x(k)/(1+x(k)^2) + 8*cos(1.2*(k-1)) + w % z(k) = x(k)^2/20 + v N = 100; % 仿真步数 Q = 1; % 过程噪声方差 R = 1; % 观测噪声方差 % ===== UKF参数 ===== n = 1; % 状态维度 alpha = 1e-3; % Sigma点分布范围参数 beta = 2; % 高斯分布最优参数 kappa = 0; % 次级缩放参数 lambda = alpha^2*(n+kappa) - n; % 计算权重 Wm = zeros(2*n+1,1); Wc = zeros(2*n+1,1); Wm(1) = lambda/(n+lambda); Wc(1) = lambda/(n+lambda) + (1-alpha^2+beta); for i = 2:2*n+1 Wm(i) = 1/(2*(n+lambda)); Wc(i) = 1/(2*(n+lambda)); end % ===== 构造仿真数据 ===== x_true = zeros(1,N); z = zeros(1,N); x_true(1) = 0.1; z(1) = x_true(1)^2/20 + sqrt(R)*randn; for k = 2:N x_true(k) = 0.5*x_true(k-1) + 2.5*x_true(k-1)/(1+x_true(k-1)^2) ... + 8*cos(1.2*(k-1)) + sqrt(Q)*randn; z(k) = x_true(k)^2/20 + sqrt(R)*randn; end % ===== UKF滤波主循环 ===== x_ukf = zeros(1,N); x_ukf(1) = 0.5; % 估计初值故意给偏 P = 1; % 协方差初值 for k = 2:N % ---- 预测阶段 ---- A = chol((n+lambda)*P, 'lower'); X = zeros(n, 2*n+1); X(:,1) = x_ukf(k-1); for i = 1:n X(:,i+1) = x_ukf(k-1) + A(:,i); X(:,i+1+n) = x_ukf(k-1) - A(:,i); end for i = 1:2*n+1 X(:,i) = 0.5*X(:,i) + 2.5*X(:,i)/(1+X(:,i)^2) + 8*cos(1.2*(k-1)); end x_pred = sum(Wm.*X, 2); P_pred = Q; for i = 1:2*n+1 d = X(:,i) - x_pred; P_pred = P_pred + Wc(i) * (d*d'); end % ---- 更新阶段 ---- A2 = chol((n+lambda)*P_pred, 'lower'); Y = zeros(n, 2*n+1); Y(:,1) = x_pred; for i = 1:n Y(:,i+1) = x_pred + A2(:,i); Y(:,i+1+n) = x_pred - A2(:,i); end Z = zeros(1, 2*n+1); for i = 1:2*n+1 Z(i) = Y(:,i)^2/20; end z_pred = sum(Wm.*Z, 2); S = R; Pxz = 0; for i = 1:2*n+1 dz = Z(i) - z_pred; S = S + Wc(i) * (dz*dz'); dx = Y(:,i) - x_pred; Pxz = Pxz + Wc(i) * (dx*dz); end K = Pxz / S; x_ukf(k) = x_pred + K * (z(k) - z_pred); P = P_pred - K * S * K'; end % ===== 误差统计 ===== rmse_ukf = sqrt(mean((x_true - x_ukf).^2)); fprintf('UKF RMSE = %.4f\n', rmse_ukf);3.3 关键代码段：预测与更新到底做了什么
拆解上面这段代码，重点就三步。
第一步是生成Sigma点。这里我用chol((n+lambda)*P, 'lower')得到矩阵平方根，注意是用下三角Cholesky分解，而不是sqrtm。sqrtm求的是矩阵的“真平方根”，计算量大且结果可能不对称，chol速度快、结果稳定，工程上几乎都用它。
第二步是状态传播。每个Sigma点独立代入状态方程，得到一组新的点，再按权重合成先验均值和先验协方差。这里有个初学者很容易漏的细节：过程噪声Q应该加在协方差预测里，不是加在Sigma点传播上。因为加性噪声假设下，噪声不影响Sigma点本身，只影响协方差的叠加。
第三步是观测更新。注意这里一定要用先验均值x_pred重新生成一组Sigma点，再代入观测方程。很多初学者偷懒直接用预测阶段的X，结果新息协方差算出来是错的，滤波器性能大打折扣。标准流程是：预测一次→重新采样→观测传播→计算增益→更新。
3.4 如何验证代码正确
代码跑通之后，先别急着拿去用。我习惯先做三步验证：一是看估计曲线和真实曲线的贴合程度，初期几个点允许有误差，但很快要收敛跟随；二是看RMSE量级，和过程噪声Q、观测噪声R的比值做直觉对比，如果误差比噪声还小而观测又好，正常；三是把估计初值故意给偏，观察滤波器能否在几步之内拉回来——这一步能验证滤波器的收敛性，比直接用真值初始化更有意义。
4. 和EKF对比：同一个例子，差距有多大？
4.1 EKF的线性化过程
拿同一个模型写EKF，需要先算雅可比矩阵。状态方程的雅可比是：
观测方程的雅可比是：
如果这两个导数在某一步出现极端值——比如趋近于0或者很大——EKF的协方差传播就会出现异常。更麻烦的是，每次算雅可比都要小心检查对不对，一旦漏了某个复合函数求导，结果会非常诡异。
4.2 对比实验结果解读
我在相同数据下用同一组初值分别跑了EKF和UKF。在我这个随机种子下，EKF的RMSE大约是0.38，UKF的RMSE大约是0.21。位置误差曲线也表明，EKF在状态转折比较剧烈的地方会出现明显滞后和尖峰，而UKF的误差整体更平稳，波动幅度更小。
这个结果其实可以直观理解：状态方程中那个分数项在x接近零时变化非常剧烈，EKF用切线近似会丢掉大量曲率信息，而UKF的Sigma点能“感知”到更宽范围内的函数形态，估算出来的均值明显更准。
4.3 什么场景应该坚持用EKF
URF也不是万能的。如果系统非线性很弱，比如只是在某个稳态工作点附近做小范围调节，EKF和UKF的差别可以忽略，这时候EKF代码简单、计算量更小，完全够用。另外，如果状态维度特别高（比如50维以上），UKF需要的Sigma点数量也会迅速增加，协方差矩阵的Cholesky分解代价上来，这时候可以考虑降维或者直接用EKF。我个人的原则是：模型弱非线性用EKF，强非线性优先UKF，实在不行再上粒子滤波。
5. 跑代码必踩的坑与排查技巧
5.1 Cholesky分解报错：协方差非正定怎么办
最常见的问题就是chol报错，提示矩阵必须为正定矩阵。原因通常是协方差矩阵在数值迭代中失去对称正定性。我排查时先做三件事：一是检查P是否对称，加一行P = (P+P')/2;强制对称；二是检查Q和R是否太小，数值精度导致协方差变成半正定；三是检查权重是否出现负值，尤其是kappa取负值的时候。
如果确认协方差确实非正定，可以用特征值裁剪修复：
[V, D] = eig(P); D = diag(max(diag(D), 1e-6)); P = V * D * V';这个方法不完美但很实用，能让滤波继续跑下去。当然，治本的办法还是从参数和模型上解决，别让协方差病态。
5.2 估计结果发散或者“过于平滑”
发散的一个常见原因是过程噪声Q给得太小。Q本质上是告诉滤波器“模型有多不可信”，如果Q趋近于0，滤波器就会完全相信状态方程，量测稍微有偏差就被当噪声滤掉，长期下来估计值就跟真实轨迹越走越远。反过来，Q给太大，滤波器又会过度相信跳变的量测，估计曲线抖得跟噪声一样。“过于平滑”的另一端还有R给得太大的情况，滤波器认为量测不可信，索性不怎么更新。遇到这种情况，我通常先用经验法则试一组Q和R，再根据残差序列的白噪声特性慢慢调。
5.3 忘记重新采样，新息算错还找不到原因
这是新手最容易犯的错误。预测阶段和更新阶段必须各做一次Sigma点采样：第一次针对k-1时刻的状态协方差，第二次针对k时刻的先验协方差。如果复用第一组Sigma点去算量测传播，交叉协方差Pxz会算偏，增益K自然也不对，最直接的表现是滤波器收敛速度变慢、误差大但又不完全发散，非常难排查。
5.4 从SOC估计到参数自整定：UKF的工程扩展
UKF的实用场景远不止教学Demo。锂电池SOC估计是这些年特别热的方向——电池的OCV与SOC之间存在明显的滞回非线性，用UKF做状态估计比EKF更稳；目标跟踪里雷达量测的距离和方位角换算成直角坐标，也是UKF的典型应用。
扩展到机器学习和数据驱动方向，很多人会把UKF和深度网络结合，比如用长短期记忆网络（LSTM）或双向LSTM建立电池模型，再用UKF做状态后处理融合。这种情况下UKF仍负责不确定性的传播，很自然地填补了纯数据方法“没有物理约束”的短板。如果嫌Q、R这些参数调起来麻烦，还可以用改进的麻雀搜索算法（ISSA）这类智能优化算法，在离线数据上自动寻优得到一组相对合理的滤波参数——我试过几次，虽然每次优化耗时不算短，但比手工试凑要靠谱得多。
在我实际跑这些例子的过程中，最大的体会是UKF的数学看着绕，代码实现却非常规整。真正吃透它的标志不是背下公式，而是能自己从零写出完整的滤波循环，并且知道每一步在做什么、为什么这么做。跑通这篇博客的代码之后，建议你把alpha从1e-3慢慢调到1，看看Sigma点的散布怎么影响误差曲线；再试着手动把模型换成自己的实际问题，体会一下从一维到多维的推广过程——这套功夫练下来，你对滤波算法的理解会比看十遍教材都深。
本文还有配套的精品资源，点击获取
