进一步理解无迹卡尔曼滤波（UKF）

无迹卡尔曼滤波（Unscented Kalman Filter, UKF），是卡尔曼滤波家族中处理非线性问题的一个成员，通过一种无迹变换的采样方法取代了EKF粗糙的线性化（泰勒展开）过程，它更准确、更鲁棒，并且更容易实现（无需推导令人头疼的雅可比矩阵），是现代非线性状态估计的首选工具之一。 在SLAM、机器人、目标跟踪和自动驾驶等领域，UKF 及其变体（如 Error-State EKF）已经很大程度上取代了传统的 EKF。

1. 解决的核心问题

UKF 的核心使命是解决 EKF 的第一个主要缺陷：线性化误差。

EKF 的问题回顾： EKF 通过一阶泰勒展开（求雅可比矩阵）在单个点（当前状态估计的均值）上进行线性化，这种近似对于高度非线性的系统来说非常粗糙，会导致：

• 引入显著的误差： 线性化截断了高阶项，近似本身就不准确。
• 导致滤波器发散： 如果系统非线性很强，这个误差会累积，最终使滤波器估计值完全偏离真实状态，彻底失效。
• 需要推导雅可比矩阵： 数学推导复杂，工程实现容易出错。

UKF 的解决方案： UKF 采用了一种完全不同的思路，与其用一个粗糙的线性函数去近似一个非线性变换，不如精心选择一组样本点（称为Sigma点），让这组点完全捕获当前状态分布的均值和协方差。然后把这组点直接通过真实的非线性函数进行变换，再从变换后的这组新的点中重新计算出新的均值和协方差。这种方法比线性化更精确，因为它更好地捕获了非线性变换后状态分布的真实均值和协方差，特别是其扭曲和变形。

2. 核心原理：无迹变换 ( UT)

UKF 的基石是无迹变换 (Unscented Transformation，UT)。它是一种计算随机变量经过非线性变换后统计特性的方法。

直觉理解： 假设模型的状态估计是一个高斯分布（一个“云团”），EKF 的做法是只看这个云团的中心点，然后假设整个云团都按照中心点的切线方向移动（满足高斯分布），而 UKF 的做法分三步实现：

1. 选点： 根据当前高斯分布的均值和协方差，精心挑选一小组有代表性的点（Sigma点）。这些点位于均值的不同方向上，距离的远近由协方差决定。
2. 传播： 将每一个Sigma点都通过真实的、非线性的函数 f 或 h 进行变换，这个过程没有任何近似，是精确传播。
3. 重构： 根据传播后的一堆新的点，重新计算出一个新的高斯分布（新的均值和协方差）。

这个新的分布比 EKF 通过线性化近似得到的分布准确得多，因为它包含了非线性效应的真实影响。

3. 实现步骤详解

UKF 的流程同样遵循传统卡尔曼滤波（KF）的预测-更新的框架，但核心操作从矩阵运算变成了对 Sigma 点的操作。

定义（与EKF相同）：

x：状态均值向量（例如：机器人的 [x坐标, y坐标, 朝向角度θ]） P：状态估计的不确定性（状态协方差矩阵） u：控制输入（例如：速度v和角速度ω） z：实际测量值（例如：到某个路标的距离r和角度φ） Q：过程噪声协方差（运动模型的不确定性） R：观测噪声协方差（传感器的不确定性） f：非线性状态转移函数 h：非线性观测函数 L：状态的维度 （UKF新增）

3.1 准备: Sigma点生成策略 首先，需要一个从当前高斯分布 (x, P) 生成 Sigma 点的策略（最常用的是对称采样策略），生成 2L+1 个 Sigma 点 Xi 及其对应的权重 Wi(m)（用于计算均值）和 Wi(c)（用于计算协方差）：

W(m)(均值权重：Weights for Mean)：用于计算变换后的Sigma点的加权平均值，以作为新分布的均值估计。中心点 X0的权重 W0(m) 通常是特殊的（可能为负或很大），而其他点的权重相，这确保了加权求和的合理性。

W(c) (协方差权重：Weights for Covariance)：用于计算变换后的Sigma点围绕新均值的加权协方差，以作为新分布的协方差估计。中心点X0的权重 W0(c)包含了参数 β，它可以调整对中心点误差的重视程度，从而提高协方差计算的精度，尤其是在分布具有较强非线性变换时（如果知道系统非线性很强，变换后分布的真实协方差与通过Sigma点计算的有差异，通过设置 β>0（通常为2），可以微调 W0(c)，使得最终计算出的协方差更接近真实值）。

α： 主要缩放参数，决定Sigma点围绕均值的扩散程度，控制 Sigma 点的分布范围（通常取一个很小的正数，如 1e-3，α 越小，Sigma点越靠近均值；α 越大，点分布得越远，以捕获更高阶的非线性效应。），取值0<α≤1。

κ： 次要缩放参数，通常设为 0，在大多数情况下，它的影响很小，保持默认值 0 即可。

β： 用来融入分布的先验知识（对于高斯分布，β=2是最优的），它可以优化协方差计算的精度，特别是在处理尾部概率时。

在实际应用中，对于高斯问题，参数通常设为 α=1e−3,β=2,κ=0α=1e−3,β=2,κ=0。

步骤 1: 预测

生成Sigma点： 基于 k-1 时刻的后验估计 x_{k−1∣k−1}和 P_{k−1∣k−}，按照上述策略生成一组 Sigma 点 X_k−1。

（关键区别） 通过状态转移函数传播Sigma点，每个 Sigma 点都通过非线性函数 f 进行变换，没有任何近似。

计算预测先验状态和协方差：

1.1 预测状态均值： 对传播后的点进行加权求和。

1.2 预测状态协方差： 计算加权协方差，并加上过程噪声 Q。

3.3 步骤 2: 更新

为预测值生成新的Sigma点（可选但常用）： 使用预测到的先验分布 (x^k∣k−1,Pk∣k−1)再生成一组新的 Sigma 点 Xk∣k−1，有些人会直接用上一步传播后的点，但重新采样更准确。

通过观测模型传播Sigma点（再次将 Sigma 点通过非线性观测函数 h 进行变换）：

预测观测均值： 对观测点进行加权求和

计算协方差：

观测协方差矩阵 P_{zz}： 预测观测值的不确定性。

状态与观测的互协方差矩阵 Pxz（状态和观测之间的关系）

2.1 计算卡尔曼增益：

4. UKF 对比 EKF 优势

5. Sigma点含义及计算示例

感性理解 想象一个二维高斯分布（像一个椭圆形的云团），其中

X0： 就是这个云团的中心点（均值）。

Xi和Xi+n： 是沿着这个椭圆主轴方向（由协方差矩阵 P 的特征向量决定）的一对对称点。它们到中心的距离由主轴的长度（特征值）和参数α,κ共同决定。

目标： 用最少的点，精确地捕捉到这个高斯分布的一阶矩（均值） 和二阶矩（协方差）。这 2L+1 个点就是这个分布的代表。

计算示例 假设正在估计一个机器的温度，

状态 x： 温度。维度 L = 1。

当前假设： 我们估计温度最可能是 25°C，但有一些不确定性，这是一个高斯分布：

均值 x = 25

方差 P = 4 (标准差 σ = 2°C)，协方差矩阵此时就是一个数字 4。

参数设置： α=1,β=2,κ=0α=1,β=2,κ=0，这是一个标准设置，便于计算：

第1步：生成Sigma点 我们需要生成：2 * L+1 = 2 *1 + 1 = 3 个Sigma点。

计算平方根项：

这三个点 [23, 25, 27] 完美代表了我们关于温度的高斯信念 N(25, 4)。它们的均值是25，方差是 ((23−25)2+(27−25)2)/2=(4+4)/2=4，与原始分布一致。

第2步：计算权重

第3步：非线性变换与重构 假设温度传感器有一个非线性误差：它的读数 z 和真实温度 x 的关系是

UKF的核心： 将每个Sigma点通过这个真实的非线性函数进行变换：

第4步：EKF对比分析 函数计算出来是62.5，为什么UKF预测的观测均值是62.9？

UKF没有只计算 h(25)=62.5，它处理的是整个概率分布，它知道温度不确定，温度不一定是正好25°，而是一个以25为中心，标准差为2的分布（即23°C到27°C都有可能）。它评估了所有可能性： 它计算了23°C、25°C、27°C分别对应的读数（52.9, 62.5, 72.9），非线性带来的不对称性：

• 温度从25°C降到23°C（变化-2°C），读数从62.5降到52.9（变化-9.6）。
• 温度从25°C升到27°C（变化+2°C），读数从62.5升到72.9（变化+10.4）。
• 下降和上升的幅度是不同的！ 这种不对称性意味着变换后的分布不是关于62.5对称的。

它计算了新的重心（均值）： UKF通过加权平均发现，由于上升效应比下降效应更强（72.9 - 62.5 > 62.5 - 52.9），整个变换后的分布的重心会向右拉，所以均值（62.9）会比正中心的变换值（62.5）略大，62.9 这个值才是对“传感器读数最可能的值”的更准确的预测，它包含了不确定性信息和非线性效应。

反观EKF，会使用一阶泰勒展开在 x=25 处线性化：

• 计算雅可比矩阵（导数）： H_j = dh/dx = 2x/10 = 5 (在x=25处)
• 预测观测值： \hat{z} = h(25) = 62.5
• 预测观测不确定性： P_{zz} = H_j * P * H_j^T = 5 * 4 * 5 = 100 EKF忽略了下行和上行效应的不对称性，它预测的读数分布会是一个以62.5为对称中心的分布。而UKF则计算出一个以62.9为中心、形状不对称的分布（方差为100.32）

第5步 区分“状态”和“观测”

这是理解SLAM和状态估计的关键，在我们的例子中，有两个不同的量：

• 状态 (State)x： 我们想要估计的真实物理量，即机器的真实温度（25°C）。
• 观测 (Measurement)z： 传感器测量并返回的读数。由于传感器模型是非线性的，这个读数（62.9）不是温度，而是一个与温度成平方关系的原始数据。

UKF（或EKF）的任务，正是利用这个奇怪的、非线性的观测读数 z=62.9（以及其不确定性），结合模型，来反过来修正我们对真实状态 x=25°C 的信念。它建立了一座连接“观测空间”和“状态空间”的桥梁。

这个简单的例子展示了UKF的完整核心流程：选点 -> 赋权 -> 非线性传播 -> 加权重构，在高维空间中，这个优势会更加明显。