企业官网建设流程全解析

看过太多关于无迹变换UT的教程，开头都是"UT是一种非线性滤波方法，通过选取sigma点……"——然后直接甩公式。我第一次接触无迹变换的时候就是在这样的文档里绕了整整两天，直到自己动手把一个两轮差速小车的位姿估计跑通，才真正明白它到底在干什么。这篇就把我从无迹变换UT到完整无迹卡尔曼滤波（UKF）的整套理解和踩过的坑摊开来讲，包括sigma点到底怎么来的、α和β这两个参数为什么不能乱设、Cholesky分解崩了怎么办、角度状态量怎么处理。如果你正在做机器人定位、传感器融合、目标跟踪，或者只是被EKF的雅可比矩阵折磨到怀疑人生，这篇应该能帮你少走点弯路。基础只需要你会求导、知道协方差矩阵是什么，剩下的我尽量用生活化的方式说清楚。

1. 为什么线性化不够用：从一个两轮差速小车的位姿估计说起

先讲个真实的场景。我手上有个两轮差速小车，状态是[x, y, θ]，用的是轮式里程计加一个外部的距离方位传感器。最开始我用的是最经典的EKF方案：在每个时间步，把非线性的运动模型和量测模型在当前估计点做一阶泰勒展开，求出雅可比矩阵，然后套用标准卡尔曼滤波的那五个公式。小车走得慢、转得缓的时候，这套东西跑得挺稳，误差也压得住。

问题出现在小车快速原地旋转的时候。那段时间估计出来的θ开始发散，位置也跟着漂，协方差矩阵的对角线元素飙到离谱的值。我查了很久，一开始以为是轮子打滑，后来把真值拿出来对比才发现，是雅可比矩阵在强非线性区域完全失真了——它是在某一个点上用一条直线去近似一条弯曲的函数曲线，当状态的分布范围一宽，这条切线就代表不了整片分布。

1.1 EKF的雅可比矩阵在强非线性下的失效现场

这里的关键在于一阶泰勒展开本质上是一个局部近似。EKF的核心假设是：状态的不确定性足够小，小到可以用一个线性映射去近似非线性函数。函数在当前点附近几乎就是直的，所以线性化误差可以忽略。

但这个假设在两类情况下会直接崩掉。第一类是函数本身曲率很大，比如sin、cos、平方、开方这些；第二类是状态的协方差本身很大，也就是我们对自己的估计没什么信心。小学数学告诉我们，用一条切线去近似一段弧，弧越长、弯得越厉害，误差越大。EKF的线性化误差，本质上就是这个"切线离弧有多远"的问题。

更麻烦的是，误差不是简单地叠加进去就完事。你在预测步丢掉的信息，到更新步会被卡尔曼增益放大，然后反映成一个偏离真实值越来越远的估计。旋转场景之所以是重灾区，就是因为θ的更新本身带着三角函数，而θ的协方差在快速旋转时会变大，两个条件同时满足。

1.2 无迹变换的核心赌注：用确定性采样点逼近分布

无迹变换的思路换了赛道：它不去线性化函数，而是去近似分布本身。

具体来说，它不再用一个点加一条切线来代表整个状态，而是精心挑选一组确定性的采样点（就是所谓的sigma点），让这组点的样本均值、样本协方差，精确地等于我们原来的状态估计均值和协方差。然后把这组点逐个丢进非线性函数里做真实的传播，再把这组传播后的结果重新加权统计，得到新的均值和协方差。

这个手法有个很漂亮的性质：它至少能精确到非线性函数的二阶矩，也就是对"弯曲"这件事的捕捉比一阶线性化强。对于高斯分布输入，UT计算出来的均值和协方差，精度能到三阶导数级别。而且它完全不需要求导——你只要能把函数当黑箱调用就行。这一点在我后来接手一些模型复杂、雅可比手推都推不出来的项目时，简直是救命稻草。

代价当然也有。你要额外计算2n+1个点（n是状态维度）的非线性函数值，计算量随维度线性增长；另外那三个尺度参数 α、β、κ 需要调，调不好会出各种奇怪问题。这些后面会细说。

2. Sigma点是按协方差椭圆挑出来的，不是随机撒点

很多人第一次看UT的公式都会有个疑问：为什么是2n+1个点？为什么点要对称？这不是随便定的，它背后是一套非常讲究的几何逻辑。理解了这一层，后面所有公式你就不用死记了。

2.1 Cholesky分解给出协方差椭圆的半轴

先把状态想清楚。一个 n 维的状态，均值和协方差构成的是一个"椭球"——二维情况下就是一个椭圆。均值的含义是这个椭圆的位置中心，协方差矩阵则决定了这个椭圆的形状、朝向和大小。

我们要挑的点，就得分布在这个椭圆上。怎么找椭圆的"骨架"？答案是矩阵开方。对协方差矩阵 P 做 Cholesky 分解，得到下三角矩阵 S，满足P = S·Sᵀ。这个 S 的每一列，就对应协方差椭圆的一条半轴方向，长度也包含了对应的尺度信息。

实际操作里我们还会乘一个缩放因子(n+λ)，也就是对(n+λ)·P做 Cholesky，把这些半轴按参数缩放后再取列。这样得到的就是一组"带权重的半轴向量"，正好拿来生成对称的采样点。

提示：Cholesky分解要求矩阵正定。如果你传进去的协方差因为是数值误差导致轻微非正定，Cholesky会直接抛异常。这是UT落地时最常遇到的第一个拦路虎，第5节会专门讲怎么救。

2.2 2n+1个点的几何意义

点是怎么落下来的？很简单：

• 一个点放在椭圆中心，也就是均值本身。
• 接下来沿着每一条半轴，往正方向走一步放一个点，再往负方向走一步放一个点。

n 维有 n 条半轴，正负各一个就是 2n 个点，加上中心点，正好2n+1个。二维情况下就是 5 个点，三维就是 7 个点。这就是那个神秘数字的来历。

这种布置方式的好处是对称性。中心点加上成对出现的正负点，天然保证了这些点的样本均值（在正确的权重下）恰好还原原均值，样本协方差也恰好还原原协方差。这不是巧合，是刻意设计出来的。

2.3 α、β、κ三个尺度参数到底在调什么

这三个参数是新手最容易糊涂的地方。我尽量讲人话。

κ（kappa）：一个自由参数，用来调整中心点相对其他点的相对权重。常见做法是取κ = 0，或者在高斯假设下取κ = 3 - n（n大于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 完整公式与逐步推导

把公式完整列一遍，方便对照：

这套流程就是UT的全部。你把它想成"给一群代表点逐个过一遍黑箱函数，然后重新统计这群体检结果"，逻辑其实很直观。

3.3 用Python手撸一遍并验证数值

光看公式容易飘，我强烈建议自己敲一遍。下面这段代码我实测跑得通：

import numpy as np def sigma_points(x, P, alpha=1e-3, beta=2.0, kappa=0.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, alpha=1e-3, beta=2.0, kappa=0.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。

我的处理方案，按推荐顺序：

1. 对称化：先做P = 0.5 * (P + P.T)。协方差理论上是对称的，但数值运算会破坏这一点，对称化能消掉一部分问题。
2. 加抖动：P = P + eps * I，eps 取 1e-9 到 1e-6 之间，根据量纲调。这一步我在90%的情况下能修好。
3. 特征值截断：如果加抖动还不行，说明真的有负特征值。做特征分解，把负特征值截到一个小正数，再重组矩阵。
4. 改用其他矩阵平方根：如果正定性问题反复出现，可以考虑用特征分解版的平方根（也就是矩阵的对称平方根），它对非正定的容忍度稍好，但计算量大一些。

提示：不要为了省事直接把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 是状态维度。听起来不错，但常数项里有两个坑：一是要算2n+1次非线性函数，如果函数本身很贵（比如要调用一个仿真器），这个开销不能忽略；二是 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值不值得上。

我个人在实际项目里的体会是，UT这套方法最大的价值不只是精度，而是它把"非线性系统滤波"这件事从"你能不能推对雅可比"变成了"你能不能调好参数"。前者是数学能力问题，后者是工程手感问题。对手推雅可比动不动就出错的项目，UT省下的调试时间远比多出来的计算量值钱。另外再分享一个小技巧：如果你不确定某个场景该不该上UT，先拿一段真实数据分别跑EKF和UKF，把两者的估计残差画出来对比。如果残差曲线的差别在噪声量级以内，那就别折腾了；如果有肉眼可见的系统性偏差，UT大概率能帮你把那块偏差啃下来。这个方法比看任何理论分析都直接。