【How Kalman Fliters Works，卡尔曼滤波（四）Sigma-Point 滤波器（Unscented Kalman Filter ，UKF）】

本文翻译自本文翻译自How Kalman Filters Work，考虑到专业水平有限，如有问题和建议，欢迎在评论区讨论！

用协方差矩阵表示不确定性

 &emsp 在Sigma-Point滤波器（也称无迹卡尔曼滤波器 unscented Kalman filters，UKF）中，不用一大堆分散的粒子来表示不确定性，而是假设不确定性呈高斯（正态）分布，并以当前最佳估计值为中心：

图1. 使用协方差矩阵（3σ 边界）与粒子进行比较

因此，我们可以用一个协方差矩阵来表示不确定性，就像上面我们计算粒子的协方差矩阵那样。如图所示，我们将协方差可视化为状态估计值周围的一个椭圆，椭圆画在$3\sigma$ 边界处（因此真实状态在 99.7% 的情况下都在这个椭圆内）。绘制 1000 个粒子只是为了便于比较。

传播

当有新的测量结果出现时，我们需要将这种不确定性向前传播到测量时刻。如何做到这一点呢？Sigma-Point滤波器的做法是，首先在当前测量值周围放置几个Sigma-Point。Sigma-Point与微粒类似，但我们对它们的位置很挑剔，而且不需要记录单个权重。Sigma-Point放置在当前估计值周围的不确定性椭圆的主轴上（通常，这个椭圆比我们绘制的要小得多）。请注意，我们只关注了问题的两个维度；在两个速度维度以及噪声维度中也有Sigma-Point，我们将在下文中详细讨论。

图2. 用Sigma-Point 代替粒子

上一个采样点 ${\hat{\mathbf{x}}}_{0,k-1}^{+}$以及对应的每个Sigma-Point ${\hat{\mathbf{x}}}_{i,k-1}^{+}$会向前传播到新的测量时刻。（加号表示根据当时的测量结果对估算值进行了修正；我们将在下文中详细介绍）。

图3. 新的测量结果和传播后的Sigma-Point

跟粒子滤波器一样，在传播过程中，Sigma-Point往往会扩散（不确定性通常会增加）。同样，我们也可以计算出Sigma-Point的加权平均值，作为新的状态估计值。
像之前一样，传递方程表示为：
$${\hat{\mathbf{x}}}_{i,k}^{-}=f\left({\hat{\mathbf{x}}}_{i,k-1}^{+},{\mathbf{q}}_{i,k-1}\right)$$
但这里有一些新东西：我们增加了一项${\mathbf{q}}_{i,k-1}$，它代表过程噪声–影响传播的未知量（比如风向变化导致的随机加速度）。我们用什么值来表示它呢？就像在状态空间中分布着Sigma-Point一样，在过程噪声空间中也分布着Sigma-Point。因此，如果有 3 个不同的因素随机影响传播，那么${\mathbf{q}}_{i,k-1}$就是一个3维向量。在Sigma-Point滤波器中，假定过程噪声是高斯的，因此用协方差矩阵$Q$表示不确定性。对于任何单模态分布来说，即使实际情况不是高斯分布，这通常也是一个很好的近似值。因此，过程噪声空间中的Sigma-Point就像状态空间中的Sigma-Point，合在一起就是Sigma-Point的大集合。

计算出Sigma-Point并传播每个Sigma-Point后，我们就可以计算出加权平均的平均预测状态：
$${\hat{\mathbf{x}}}_k^{-}=W_0{\hat{\mathbf{x}}}_{0,k}^{-}+W\sum _{i=1}^n{\hat{\mathbf{x}}}_{i,k}^{-}$$
其中${\hat{\mathbf{x}}}_{0,k}^{-}$ 为 "中间 "Sigma-Point。除了中间点，所有Sigma-Point都使用相同的权重 $W$，中间点通常使用较大的 $W_0$，因为我们更信任它。现在，我们得到了 第$k$次采样的最佳状态预测值，并用减号表示 "修正前 "的新测量值，即先验值。
然后，我们可以计算这个新估计值周围的样本协方差，得出预测状态的协方差（同样，中间点的权重 $W_c$ 比其他点大，但思路是一样的）。
P k − = W c ( x ^ 0 , k − − x ^ k − ) ( x ^ 0 , k − − x ^ k − ) T + W ∑ i = 1 n ( x ^ i , k − − x ^ k − ) ( x ^ i , k − − x ^ k − ) T P_{k}^{-} = W_c(\hat{\bm{x}}_{0,k}^{-}-\hat{\bm{x}}_{k}^{-})(\hat{\bm{x}}_{0,k}^{-}-\hat{\bm{x}}_{k}^{-})^{T} + W\sum_{i=1}^{n}(\hat{\bm{x}}_{i,k}^{-}-\hat{\bm{x}}_{k}^{-})(\hat{\bm{x}}_{i,k}^{-}-\hat{\bm{x}}_{k}^{-})^{T} Pk−​=Wc​(x^0,k−​−x^k−​)(x^0,k−​−x^k−​)