从UT到UKF：无迹变换原理、Sigma点生成与实战踩坑详解

看过太多关于无迹变换UT的教程开头都是UT是一种非线性滤波方法通过选取sigma点……——然后直接甩公式。我第一次接触无迹变换的时候就是在这样的文档里绕了整整两天直到自己动手把一个两轮差速小车的位姿估计跑通才真正明白它到底在干什么。这篇就把我从无迹变换UT到完整无迹卡尔曼滤波UKF的整套理解和踩过的坑摊开来讲包括sigma点到底怎么来的、α和β这两个参数为什么不能乱设、Cholesky分解崩了怎么办、角度状态量怎么处理。如果你正在做机器人定位、传感器融合、目标跟踪或者只是被EKF的雅可比矩阵折磨到怀疑人生这篇应该能帮你少走点弯路。基础只需要你会求导、知道协方差矩阵是什么剩下的我尽量用生活化的方式说清楚。1. 为什么线性化不够用从一个两轮差速小车的位姿估计说起先讲个真实的场景。我手上有个两轮差速小车状态是[x, y, θ]用的是轮式里程计加一个外部的距离方位传感器。最开始我用的是最经典的EKF方案在每个时间步把非线性的运动模型和量测模型在当前估计点做一阶泰勒展开求出雅可比矩阵然后套用标准卡尔曼滤波的那五个公式。小车走得慢、转得缓的时候这套东西跑得挺稳误差也压得住。问题出现在小车快速原地旋转的时候。那段时间估计出来的θ开始发散位置也跟着漂协方差矩阵的对角线元素飙到离谱的值。我查了很久一开始以为是轮子打滑后来把真值拿出来对比才发现是雅可比矩阵在强非线性区域完全失真了——它是在某一个点上用一条直线去近似一条弯曲的函数曲线当状态的分布范围一宽这条切线就代表不了整片分布。1.1 EKF的雅可比矩阵在强非线性下的失效现场这里的关键在于一阶泰勒展开本质上是一个局部近似。EKF的核心假设是状态的不确定性足够小小到可以用一个线性映射去近似非线性函数。函数在当前点附近几乎就是直的所以线性化误差可以忽略。但这个假设在两类情况下会直接崩掉。第一类是函数本身曲率很大比如sin、cos、平方、开方这些第二类是状态的协方差本身很大也就是我们对自己的估计没什么信心。小学数学告诉我们用一条切线去近似一段弧弧越长、弯得越厉害误差越大。EKF的线性化误差本质上就是这个切线离弧有多远的问题。更麻烦的是误差不是简单地叠加进去就完事。你在预测步丢掉的信息到更新步会被卡尔曼增益放大然后反映成一个偏离真实值越来越远的估计。旋转场景之所以是重灾区就是因为θ的更新本身带着三角函数而θ的协方差在快速旋转时会变大两个条件同时满足。1.2 无迹变换的核心赌注用确定性采样点逼近分布无迹变换的思路换了赛道它不去线性化函数而是去近似分布本身。具体来说它不再用一个点加一条切线来代表整个状态而是精心挑选一组确定性的采样点就是所谓的sigma点让这组点的样本均值、样本协方差精确地等于我们原来的状态估计均值和协方差。然后把这组点逐个丢进非线性函数里做真实的传播再把这组传播后的结果重新加权统计得到新的均值和协方差。这个手法有个很漂亮的性质它至少能精确到非线性函数的二阶矩也就是对弯曲这件事的捕捉比一阶线性化强。对于高斯分布输入UT计算出来的均值和协方差精度能到三阶导数级别。而且它完全不需要求导——你只要能把函数当黑箱调用就行。这一点在我后来接手一些模型复杂、雅可比手推都推不出来的项目时简直是救命稻草。代价当然也有。你要额外计算2n1个点n是状态维度的非线性函数值计算量随维度线性增长另外那三个尺度参数 α、β、κ 需要调调不好会出各种奇怪问题。这些后面会细说。2. Sigma点是按协方差椭圆挑出来的不是随机撒点很多人第一次看UT的公式都会有个疑问为什么是2n1个点为什么点要对称这不是随便定的它背后是一套非常讲究的几何逻辑。理解了这一层后面所有公式你就不用死记了。2.1 Cholesky分解给出协方差椭圆的半轴先把状态想清楚。一个 n 维的状态均值和协方差构成的是一个椭球——二维情况下就是一个椭圆。均值的含义是这个椭圆的位置中心协方差矩阵则决定了这个椭圆的形状、朝向和大小。我们要挑的点就得分布在这个椭圆上。怎么找椭圆的骨架答案是矩阵开方。对协方差矩阵 P 做 Cholesky 分解得到下三角矩阵 S满足P S·Sᵀ。这个 S 的每一列就对应协方差椭圆的一条半轴方向长度也包含了对应的尺度信息。实际操作里我们还会乘一个缩放因子(nλ)也就是对(nλ)·P做 Cholesky把这些半轴按参数缩放后再取列。这样得到的就是一组带权重的半轴向量正好拿来生成对称的采样点。提示Cholesky分解要求矩阵正定。如果你传进去的协方差因为是数值误差导致轻微非正定Cholesky会直接抛异常。这是UT落地时最常遇到的第一个拦路虎第5节会专门讲怎么救。2.2 2n1个点的几何意义点是怎么落下来的很简单一个点放在椭圆中心也就是均值本身。接下来沿着每一条半轴往正方向走一步放一个点再往负方向走一步放一个点。n 维有 n 条半轴正负各一个就是 2n 个点加上中心点正好2n1个。二维情况下就是 5 个点三维就是 7 个点。这就是那个神秘数字的来历。这种布置方式的好处是对称性。中心点加上成对出现的正负点天然保证了这些点的样本均值在正确的权重下恰好还原原均值样本协方差也恰好还原原协方差。这不是巧合是刻意设计出来的。2.3 α、β、κ三个尺度参数到底在调什么这三个参数是新手最容易糊涂的地方。我尽量讲人话。κkappa一个自由参数用来调整中心点相对其他点的相对权重。常见做法是取κ 0或者在高斯假设下取κ 3 - nn大于3时会是负的这也没问题。它影响的是中心点离其他点多远的感觉。αalpha控制采样点从均值散开的程度。α 越小点越贴近均值。但在实际使用中大家几乎都取一个很小的值比如1e-3甚至1e-4。这就导致一个副作用中心点的均值权重λ/(nλ)会变成一个很负的数而协方差权重W0^c因为带上(1 - α² β)反而是个很大的正数。这两个极端权重是后面数值问题的根源之一。βbeta用来把关于分布的先验知识塞进去。如果假设是高斯分布β 2是最优选择能提升协方差的计算精度。对于非高斯β 没有统一的最优值一般还是取 2 作为默认。把这三个参数代进λ α²(nκ) - n就得到所有权重。很多人照着公式抄从来不想这几个数为什么是这样结果一换场景就翻车。我的建议是先在标准参数下跑通确认逻辑对了再去动参数。乱调 α 和 β 带来的问题往往比你想解决的还多。3. 传播与重加权把分布搬过非线性函数的完整步骤参数定了、点选好了接下来就是让这组点真正干活。这一节我打算把整个流程拆到每一步能对着代码核对的粒度。3.1 均值权重和协方差权重为什么不一样先解决一个高频困惑为什么会有两套权重Wm和Wc均值权重Wm干的事是把传播后的点加权平均回一个中心点也就是新的均值。它要求权重加起来等于 1这样加权平均才有平均的意义。协方差权重Wc干的事就微妙了。协方差算的是点相对均值的离散程度也就是Σ Wc_i (Y_i - ȳ)(Y_i - ȳ)ᵀ。中心点本身对均值的偏离是零但它对分布的展宽是有贡献的而且这个贡献和 β 有关。为了让高斯假设下的协方差估计更准我们给中心点的协方差权重额外加上了(1 - α² β)这一项。所以两套权重只在第0个点上不同其余1到2n号点完全一样都等于1/(2(nλ))。3.2 完整公式与逐步推导把公式完整列一遍方便对照步骤公式说明计算 λλ α²(nκ) - n缩放基准生成点X₀ x̄Xᵢ x̄ ± S·col_iS 是 (nλ)P 的 Cholesky 因子均值权重W₀ᵐ λ/(nλ)Wᵢᵐ 1/(2(nλ))i ≥ 1协方差权重W₀ᶜ λ/(nλ) (1-α²β)Wᵢᶜ 同上i ≥ 1传播Yᵢ f(Xᵢ)逐个过非线性函数新均值ȳ Σ Wᵢᵐ Yᵢ加权平均新协方差P_y Σ Wᵢᶜ (Yᵢ-ȳ)(Yᵢ-ȳ)ᵀ加权外积和这套流程就是UT的全部。你把它想成给一群代表点逐个过一遍黑箱函数然后重新统计这群体检结果逻辑其实很直观。3.3 用Python手撸一遍并验证数值光看公式容易飘我强烈建议自己敲一遍。下面这段代码我实测跑得通import numpy as np def sigma_points(x, P, alpha1e-3, beta2.0, kappa0.0): n len(x) lam alpha**2 * (n kappa) - n S np.linalg.cholesky((n lam) * P) X np.zeros((2 * n 1, n)) X[0] x for i in range(n): X[i 1] x S[:, i] X[i n 1] x - S[:, i] Wm np.full(2 * n 1, 1.0 / (2 * (n lam))) Wc Wm.copy() Wm[0] lam / (n lam) Wc[0] lam / (n lam) (1 - alpha**2 beta) return X, Wm, Wc def unscented_transform(x, P, f, alpha1e-3, beta2.0, kappa0.0): X, Wm, Wc sigma_points(x, P, alpha, beta, kappa) Y np.array([f(xi) for xi in X]) y Wm Y dY Y - y Py (Wc[:, None] * dY).T dY return y, Py, Y, X, Wm, Wc拿一个二维例子验证一下比如函数f([a, b]) [a*b, np.sin(a)]输入均值[1.0, 2.0]协方差取单位阵乘0.1。跑一遍看输出的均值和协方差是否合理。我第一次跑的时候因为把 Cholesky 写在了错误的矩阵上结果均值偏了一大截还以为是算法本身有问题。所以验证时一定要用蒙特卡洛大样本去对照撒一万个点过函数统计均值和协方差然后跟你UT算出来的比。两者接近说明实现没错。注意验证的时候别用太小的 α。α 取 1e-3 时中心点的均值权重会是很负的大数单看输出会有点反直觉但整体统计量是对的。这个现象本身不是bug。4. 从UT到UKF预测步和更新步的分工UT解决了怎么把分布过非线性函数但滤波还需要两个动作用上一时刻的状态往前推进预测以及用新来的观测修正估计更新。把UT嵌进这两个动作就是UKF。4.1 预测步状态sigma点过过程模型预测步的逻辑是拿当前的均值x和协方差P生成一组sigma点把这组点逐个过状态转移函数f(x, u)u 是控制量然后加回过程噪声Q。1. 从 (x, P) 生成 sigma 点 X 2. 每个点过过程模型X_pred_i f(X_i, u) 3. 加权得到预测均值x_pred Σ Wm_i · X_pred_i 4. 加权得到预测协方差P_pred Σ Wc_i · (X_pred_i - x_pred)(...)ᵀ Q关键点在最后加Q过程噪声是在传播之后叠加的因为噪声是作用在新的时刻上的。Q 的具体形式取决于你的噪声建模简单的话就是对角阵加一个小量。4.2 更新步量测sigma点与交叉协方差更新步要稍微绕一点。这里的技巧是用预测出来的状态x_pred, P_pred重新生成一组sigma点然后再过一次量测函数 h而不是复用预测步的老点。因为预测之后的协方差已经变了必须重新采样才能反映新的不确定度。生成新点之后1. 从 (x_pred, P_pred) 生成 sigma 点 X 2. 每个点过量测模型Z_i h(X_i) 3. 加权得到预测观测z_pred Σ Wm_i · Z_i 4. 加权得到观测协方差P_zz Σ Wc_i · (Z_i - z_pred)(...)ᵀ R 5. 加权得到交叉协方差P_xz Σ Wc_i · (X_i - x_pred)(Z_i - z_pred)ᵀ交叉协方差P_xz是UKF里最容易被忽略但最重要的量。它记录的是状态和观测之间的关联是卡尔曼增益的核心输入。线性系统里这个量对应的是P·Hᵀ非线性情况下UT直接用样本外积把它估出来了省去了求雅可比。得到增益和更新K P_xz · inv(P_zz) x_upd x_pred K · (z_meas - z_pred) P_upd P_pred - K · P_zz · Kᵀ4.3 卡尔曼增益的骨架为什么还能用有人会问整个流程都非线性了为什么最后一步还是那套线性卡尔曼的更新公式原因在于卡尔曼滤波的更新步骤本身并不要求线性它要求的是状态和观测之间的关系可以用协方差描述。UT给出的是传播后分布的均值和协方差虽然分布本身可能已经不正态了但我们用高斯去近似它然后在这个高斯假设下应用最优线性更新。换句话说非线性只是被局部高斯化了更新的数学结构保持不变。理解这一点很重要。它意味着UKF不是万能的——如果传播后的分布严重歪斜比如双峰用高斯去套会丢信息。这时候得考虑粒子滤波这类方法。但在绝大多数工程场景里UT的近似够用。5. 实战踩坑矩阵开方、角度状态量和维度代价理论讲完进入最实用的部分。下面这几个坑我几乎在每个UKF项目里都会至少撞上一个。5.1 Cholesky失败协方差非正定怎么救UT的第一步就是Cholesky分解而它要求矩阵正定。问题在于经过大量迭代后浮点误差会让协方差矩阵出现微小的负特征值Cholesky直接报错LinAlgError: Matrix is not positive definite。我的处理方案按推荐顺序对称化先做P 0.5 * (P P.T)。协方差理论上是对称的但数值运算会破坏这一点对称化能消掉一部分问题。加抖动P P eps * Ieps 取 1e-9 到 1e-6 之间根据量纲调。这一步我在90%的情况下能修好。特征值截断如果加抖动还不行说明真的有负特征值。做特征分解把负特征值截到一个小正数再重组矩阵。改用其他矩阵平方根如果正定性问题反复出现可以考虑用特征分解版的平方根也就是矩阵的对称平方根它对非正定的容忍度稍好但计算量大一些。提示不要为了省事直接把np.linalg.cholesky换成伪逆或者随便糊弄过去。协方差不正定往往是滤波发散的早期信号掩盖它只会让问题在后面爆得更狠。加抖动的同时一定要回头看看是不是 Q 或 R 设得太小。5.2 角度量的wrap-around处理只要你的状态里有角度机器人朝向、航向角就会撞上这个问题角度在 π 和 -π 之间跳变。如果你直接让状态点在某条半轴上从3.1走到3.2它其实应该绕回-3.08但线性计算不会帮你处理。处理办法有两种。一是状态增广把角度用sin和cos两个量表示消除跳变用的时候再atan2还原。代价是状态维度增加计算量变大。二是残差归一化在计算观测残差z_meas - z_pred时把角度差归一化到[-π, π]。这个方法简单但只在角度出现在观测残差里时有效如果角度在状态传播里有强烈非线性还是推荐增广。我在小车项目里两个方法都用过。最后选了增广因为状态维度小多出来的计算量可以接受而且省去了到处做归一化的麻烦。5.3 高维状态下的参数取舍与计算量UT的计算量是 O(n)n 是状态维度。听起来不错但常数项里有两个坑一是要算2n1次非线性函数如果函数本身很贵比如要调用一个仿真器这个开销不能忽略二是 Cholesky 分解是 O(n³)。状态维度上到二三十维时你会明显感觉到帧率掉了。这时候的取舍是能降维就降维。很多状态其实可以合并或者用更紧凑的参数表示。另外α 别设得太小太小的 α 会让中心点权重极端化加剧数值问题。我一般用α 1做数值稳定性优先的配置只有在确实需要精细调优时才往小里调。6. 横向对比UT、EKF和一阶近似各在什么场景划算讲了这么多UT的优点也得说说它什么时候不划算。没有银弹选型要看场景。6.1 一个强非线性量测的对比实验我做过一个稍微正式点的对比用一个纯方位跟踪的场景state是目标位置和速度观测量是相对于观测站的方位角分别跑EKF和UKF同样的噪声参数、同样的初始条件、同一段真实轨迹。结果挺能说明问题。在目标远离观测站、方位角变化平缓的时段两者几乎没差别。但当目标靠近观测站、方位角快速变化时EKF的估计开始有可见的偏差UKF的表现明显更贴合真值。协方差方面EKF常常过度自信估出来的不确定度比实际小而UKF的协方差更接近真实误差的统计。这个实验的结论不是UKF更好而是UT的优势在强非线性区域才体现得出来。线性或者弱非线性场景下两者几乎等价而EKF计算更便宜、调试更简单。6.2 什么时候该放弃UT几种情况我会考虑不用UT状态维度非常高比如上百维Cholesky的开销吃不消这时候可能用集合卡尔曼滤波或者分块处理。分布严重非高斯比如有强多峰特性UT的高斯近似会丢关键信息得换成粒子滤波。非线性函数本身不平滑有跳变或分段UT的统计近似前提不成立。实时性要求极苛刻且场景是弱非线性EKF的线性化足够用。选型的判断标准其实就一句话你的非线性有多弯你的分布有多宽。这两点决定了线性化近似的误差有多大也就决定了UT值不值得上。方法非线性处理求导需求计算量适用场景EKF一阶线性化需要雅可比低弱非线性、实时性优先UKF/UT确定性采样黑箱即可中中强非线性、模型复杂粒子滤波蒙特卡洛黑箱即可高强非高斯、多峰分布我个人在实际项目里的体会是UT这套方法最大的价值不只是精度而是它把非线性系统滤波这件事从你能不能推对雅可比变成了你能不能调好参数。前者是数学能力问题后者是工程手感问题。对手推雅可比动不动就出错的项目UT省下的调试时间远比多出来的计算量值钱。另外再分享一个小技巧如果你不确定某个场景该不该上UT先拿一段真实数据分别跑EKF和UKF把两者的估计残差画出来对比。如果残差曲线的差别在噪声量级以内那就别折腾了如果有肉眼可见的系统性偏差UT大概率能帮你把那块偏差啃下来。这个方法比看任何理论分析都直接。