无迹卡尔曼滤波器（Unscented Kalman Filter，简称 UKF）是目前处理非线性系统时最受欢迎、最实用的高斯滤波方法之一

无迹卡尔曼滤波器Unscented Kalman Filter简称 UKF是目前处理非线性系统时最受欢迎、最实用的高斯滤波方法之一。它被广泛认为是“比 EKF 更好、更稳定”的替代方案尤其在机器人、无人机、传感器融合、目标跟踪、电池 SOC 估计、功率器件热模型等领域应用非常广泛。UKF vs EKF 快速对比为什么很多人直接跳过 EKF 用 UKF项目Extended Kalman Filter (EKF)Unscented Kalman Filter (UKF)谁胜出处理非线性方式一阶泰勒展开线性化 雅可比矩阵无迹变换Unscented Transform → 采样 Sigma 点UKF精度强非线性容易引入大误差低估不确定性通常达到二阶甚至更高精度UKF稳定性/发散风险较高线性化误差累积显著更低无需求导不近似函数UKF是否需要雅可比是手动或自动求导否只用函数 f 和 hUKF计算量较低O(n³)n状态维中等O(n³)但常数项更大粒子数固定 ~2n1EKF 稍快噪声假设加性高斯加性高斯可扩展到非加性平手实现难度中等求雅可比麻烦中等偏高Sigma 点 权重计算平手实际工程首选简单非线性、计算资源极紧大多数现代项目IMU、SLAM、Autopilot 等UKF一句话总结UKF 几乎在所有场景下都比 EKF 更准确、更鲁棒除非你对计算资源极端敏感。UKF 核心思想最通俗解释EKF 是“把非线性函数在一点附近拉直”容易出错。UKF 是“聪明地选几个代表点Sigma points把这些点扔进真正的非线性函数里跑一遍再把结果加权平均回来”。这些 Sigma 点不是随机采样的而是精心设计的1 个中心点当前均值2n 个沿每个维度 ±√(nκ)λ 的偏移点n 状态维度通过这些点你能精确捕捉到均值和协方差的二阶信息甚至部分更高阶而不需要求导。UKF 算法步骤简洁版生成 Sigma 点2n1 个点从当前 x 和 P 计算 χᵢSigma 点矩阵预测阶段Time Update对每个 Sigma 点通过非线性过程函数 f χᵢ⁻ f(χᵢ, u)计算预测均值 x⁻ 和预测协方差 P⁻加过程噪声 Q观测更新Measurement Update对预测的 Sigma 点通过非线性观测函数 h ζᵢ h(χᵢ⁻)计算预测观测均值 z⁻、观测协方差 P_{zz}、交叉协方差 P_{xz}卡尔曼增益 K P_{xz} P_{zz}⁻¹更新x x⁻ K (z - z⁻)P P⁻ - K P_{zz} Kᵀ实用 C# 实现基于 MathNet.Numerics低维示例以下是一个二维状态位置 速度的 UKF 实现观测是非线性的例如 z p² 噪声。usingMathNet.Numerics.LinearAlgebra;usingMathNet.Numerics.LinearAlgebra.Double;usingSystem;publicclassUnscentedKalmanFilter{publicVectordoubleState{get;privateset;}// xpublicMatrixdoubleCovariance{get;privateset;}// PprivatereadonlyMatrixdouble_Q;// 过程噪声协方差privatereadonlydouble_R;// 测量噪声假设单观测标量privatereadonlydouble_dt;// UKF 参数经典默认值privatereadonlydouble_alpha1e-3;// 控制 Sigma 点分布privatereadonlydouble_beta2.0;// 通常 2最优高斯privatereadonlydouble_kappa0.0;// 通常 0 或 3-nprivateintnState.Count;// 状态维度publicUnscentedKalmanFilter(doubleinitPos,doubleinitVel,doubleinitPosUnc,doubleinitVelUnc,doubleprocessNoiseAccel,doublemeasNoise,doubledt0.1){StateVector.Build.DenseOfArray(new[]{initPos,initVel});CovarianceMatrix.Build.Diagonal(new[]{initPosUnc,initVelUnc});_QMatrix.Build.DenseOfArray(new[,]{{processNoiseAccel*dt*dt*dt*dt/4,processNoiseAccel*dt*dt*dt/2},{processNoiseAccel*dt*dt*dt/2,processNoiseAccel*dt*dt}});_RmeasNoise;_dtdt;}publicvoidPredict(){varsigmaPointsGenerateSigmaPoints();varpredictedSigmanewMatrixdouble(n,2*n1);for(inti0;i2*n1;i){doublepsigmaPoints[0,i];doublevsigmaPoints[1,i];predictedSigma[0,i]pv*_dt;// 非线性 f 可在这里替换predictedSigma[1,i]v;}// 计算预测均值和协方差(State,Covariance)ComputeMeanAndCov(predictedSigma,_Q);}publicvoidUpdate(doublemeasurement){varsigmaPointsGenerateSigmaPoints();// 基于预测后的 x⁻, P⁻varpredictedObsSigmanewMatrixdouble(1,2*n1);for(inti0;i2*n1;i){doublepsigmaPoints[0,i];predictedObsSigma[0,i]p*p;// 非线性 h(z) p²可任意替换}var(zPred,Pzz)ComputeMeanAndCovObs(predictedObsSigma);varPxzComputeCrossCov(sigmaPoints,predictedObsSigma,zPred);varSPzzMatrix.Build.DenseOfScalar(_R);varKPxz*S.Inverse();StateK*Vector.Build.DenseOfArray(new[]{measurement-zPred});Covariance-K*S*K.Transpose();}privateMatrixdoubleGenerateSigmaPoints(){doublelambda_alpha*_alpha*(n_kappa)-n;varsqrtPCovariance.Cholesky().Factor*Math.Sqrt(nlambda);varsigmaMatrix.Build.Dense(n,2*n1);sigma.SetColumn(0,State);for(inti0;in;i){sigma.SetColumn(i1,StatesqrtP.Column(i));sigma.SetColumn(i1n,State-sqrtP.Column(i));}returnsigma;}private(Vectordoublemean,Matrixdoublecov)ComputeMeanAndCov(Matrixdoublesigmas,MatrixdoublenoiseCov){doubleWm0_alpha*_alpha*(n_kappa)/(n_alpha*_alpha*(n_kappa));doubleWmi1.0/(2*(n_alpha*_alpha*(n_kappa)));varmeansigmas*Vector.Build.Dense(2*n1,ii0?Wm0:Wmi);vardiffsigmas-mean.Outer(Vector.Build.Dense(2*n1,_1.0));varcovdiff*Matrix.Build.Diagonal(2*n1,ii0?Wm0(1-_alpha*_alpha_beta):Wmi)*diff.Transpose()noiseCov;return(mean,cov);}private(doublemean,Matrixdoublecov)ComputeMeanAndCovObs(Matrixdoublesigmas){doubleWm0_alpha*_alpha*(n_kappa)/(n_alpha*_alpha*(n_kappa));doubleWmi1.0/(2*(n_alpha*_alpha*(n_kappa)));doublemean0;for(inti0;i2*n1;i)mean(i0?Wm0:Wmi)*sigmas[0,i];doublecov0;for(inti0;i2*n1;i){doublewi0?Wm0(1-_alpha*_alpha_beta):Wmi;doubledsigmas[0,i]-mean;covw*d*d;}return(mean,Matrix.Build.DenseOfScalar(cov));}privateMatrixdoubleComputeCrossCov(MatrixdoublestateSigmas,MatrixdoubleobsSigmas,doublezMean){doubleWm0_alpha*_alpha*(n_kappa)/(n_alpha*_alpha*(n_kappa));doubleWmi1.0/(2*(n_alpha*_alpha*(n_kappa)));varcrossMatrix.Build.Dense(n,1);for(inti0;i2*n1;i){doublewi0?Wm0:Wmi;vardxstateSigmas.Column(i)-State;doubledzobsSigmas[0,i]-zMean;crossw*dx.Outer(Vector.Build.DenseOfArray(new[]{dz}));}returncross;}public(doublePos,doubleVel)GetEstimate()(State[0],State[1]);}推荐工程实践直接用现成库UKFSharpGitHub: prozoroff/UKFSharp → 简单、基于 MathNetMathNet 自己写 Sigma 点逻辑上面代码风格Accord.NET 或其他数值库扩展参数调优口诀α ≈ 1e-3 ~ 1e-2小 α → Sigma 点更集中β 2高斯最优κ 0 或 3-n如果你有具体的非线性方程例如结温热模型、Vf-Tvj 非线性映射、IMU 融合等告诉我过程函数 f 和观测函数 h我可以帮你把上面的代码改成直接可用的版本。