【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({\hat{\mathbf{x}}}_{0,k}^{-}-{\hat{\mathbf{x}}}_k^{-})({\hat{\mathbf{x}}}_{0,k}^{-}-{\hat{\mathbf{x}}}_k^{-}{)}^T+W\sum _{i=1}^n({\hat{\mathbf{x}}}_{i,k}^{-}-{\hat{\mathbf{x}}}_k^{-})({\hat{\mathbf{x}}}_{i,k}^{-}-{\hat{\mathbf{x}}}_k^{-}{)}^T$$

图4. 传播后的预测状态协方差

现在，我们已经知道了测量结果出来时的状态，以及与我们的预测相关的不确定性。

修正

当然，现在我们需要利用测量信息来修正状态估计值和协方差。就像我们有每个Sigma-Point的预测状态${\hat{\mathbf{x}}}_{i,k}^{-}$和总体预测状态${\hat{\mathbf{x}}}_k^{-}$，我们通过对每个Sigma-Point进行测量，得出总体预测测量值${\mathbf{z}}_k$：
$${\mathbf{z}}_k=h({\hat{\mathbf{x}}}_{i,k}^{-},{\mathbf{r}}_{i,k})$$其中，${\mathbf{r}}_{i,k}$表示方差为$R$的测量噪声，类似过程噪声。然后计算加权平均值：
$${\hat{\mathbf{z}}}_k=W_0{\hat{\mathbf{z}}}_{0,k}^{-}+W\sum _{i=1}^n{\hat{\mathbf{z}}}_{i,k}^{-}$$我们将利用这个预测测量值来修正预测状态，就像这样：
$$\begin{array}{rl}{\mathbf{y}}_k& ={\mathbf{z}}_k-{\hat{\mathbf{z}}}_k\\ {\hat{\mathbf{x}}}_k^{+}& ={\hat{\mathbf{x}}}_k^{-}+K{\mathbf{y}}_k\end{array}$$其中$+$表示 "测量修正后 "或后验值。由于历史原因，${\mathbf{y}}_k$被称为创新向量（innovation vector）、测量预测误差（measurement prediction error）或残差（residual）。它是一个从我们预测的测量结果指向实际测量结果的向量，$K$决定了测量空间中的误差向量如何映射到状态空间中的修正。

让我们考虑一下 $K$ 的一些理想特性。如果我们对状态的了解已经非常充分，而测量结果又非常嘈杂，没有什么用处，那么 $K$ 就应该很小——我们可能不希望对预测结果做太多修正。（例如，如果有人喊 “狼来了！”，但那家伙总是在喊 “狼来了”，而且这里已经很久没有人看到过狼了，那么你就可以忽略他，或者至少大部分情况下忽略他）。另一方面，如果我们对该状态的了解非常糟糕，而测量结果已知非常好（这里经常看到狼，而 喊“狼来了”的人通常对这类事情的判断是正确的），那么我们会更愿意相信测量结果。在这种情况下，我们会希望 $K$ 很大（我们可能没有预测到有狼，但现在看来肯定有一只）。

将其转化为数学，我们用一个交叉协方差矩阵（corss-covariance matrix） $P_{xy}$ 来表示状态的不确定性（是否有狼？）和测量误差（有人错误地喊“狼来了”）之间地关系；我们用另一个协方差矩阵 $P_{yy}$ 来表示测量误差（有人错误地喊“狼来了”）的不确定性。如果我们真的确定有或没有狼，那么 $P_{xy}$ 就会非常小。如果周围没有恶作剧者，$P_{yy}$ 也会很小，但不会小于我们对周围是否有狼的基本不确定性。如果周围有恶作剧者，那么$P_{yy}$就会很大——误差很大。
$$\begin{array}{rl}{P}_{xy}& =W_c({\hat{\mathbf{x}}}_{0,k}^{-}-{\hat{\mathbf{x}}}_k^{-})({\hat{\mathbf{z}}}_{0,k}^{-}-{\hat{\mathbf{z}}}_k^{-}{)}^T+P_{xy}+\sum _{i=1}^{n}W({\hat{\mathbf{x}}}_{i,k}^{-}-{\hat{\mathbf{x}}}_k^{-})({\hat{\mathbf{z}}}_{i,k}^{-}-{\hat{\mathbf{z}}}_k^{-}{)}^T\\ P_{yy}& =W_c({\hat{\mathbf{z}}}_{0,k}^{-}-{\hat{\mathbf{z}}}_k^{-})({\hat{\mathbf{z}}}_{0,k}^{-}-{\hat{\mathbf{z}}}_k^{-}{)}^T+P_{xy}+\sum _{i=1}^{n}W({\hat{\mathbf{z}}}_{i,k}^{-}-{\hat{\mathbf{z}}}_k^{-})({\hat{\mathbf{z}}}_{i,k}^{-}-{\hat{\mathbf{z}}}_k^{-}{)}^T\end{array}$$
回到弹跳球的例子，我们测量的是球的位置，因此我们可以在同一幅图中绘制位置误差的 $P_{xy}$ 和 $P_{yy}$：

图5. P_xy 和 P_yy

利用这两个协方差矩阵，构造一个简单的比率：预测的不确定性（${\hat{x}}_k^{-}$ 和 ${\hat{z}}_k$ ）作分子；我们预测的测量误差的不确定性（${\mathbf{y}}_k={\mathbf{z}}_k-{\hat{\mathbf{z}}}_k$）作分母，表达成矩阵形式：
$$K=P_{xy}{P}_{yy}^{-1}$$事实证明，这个 "比率 "确实是个不错的选择。对于线性系统来说，一般来说，它能产生平方误差最小的估计值。这就是卡尔曼增益。

请注意，在我们的例子中，$P_{xy}$ 和 $P_{yy}$ 的椭球体非常接近。这意味着我们对球的走向有很大的不确定性，而我们对测量结果的不确定性也更大一些。由于卡尔曼增益就像一个比率，而我们的 "分子 "和 "分母 "非常接近，
因此我们的位置部分的增益将接近的单位矩阵。相应的也有一个因子将更新状态的速度部分。本例中实际的卡尔曼增益为：
$$K=\left[\begin{array}{cc}0.8& 0\\ 0& 0.8\\ 1.2& 0\\ 0& 1.2\end{array}\right]$$
其中，两个位置项是状态的上半部分，两个速度项在下半部分。这说明，更新将是预测位置与测量位置之间差值的大部分（0.8），相应地，速度也将沿同一方向增加（1.2）。

现在我们得到了当前测量的最终状态估计值：
$${\hat{\mathbf{x}}}_k^{+}={\hat{\mathbf{x}}}_k^{-}+K{\mathbf{y}}_k$$

修正协方差

修正也会影响我们新估计值的不确定性，而我们还没有更新Sigma-Point，所以我们还没有说明修正如何减少了我们的不确定性。我们可以按照修正状态的方法，修正Sigma-Point来说明这一点：
$${\hat{\mathbf{x}}}_{i，k}^{+}={\hat{\mathbf{x}}}_{i，k}^{-}+K({\mathbf{z}}_k-{\hat{\mathbf{z}}}_{i,k})$$然后，我们可以计算出新的协方差矩阵，即校正后Sigma-Point的样本协方差$P_k^{+}$：
$$P_k^{+}=W_c({\hat{\mathbf{x}}}_{0，k}^{+}-{\hat{\mathbf{x}}}_K^{+})({\hat{\mathbf{x}}}_{0，k}^{+}-{\hat{\mathbf{x}}}_K^{+}{)}^T+W\sum _{i=1}^n({\hat{\mathbf{x}}}_{i，k}^{+}-{\hat{\mathbf{x}}}_K^{+})({\hat{\mathbf{x}}}_{i，k}^{+}-{\hat{\mathbf{x}}}_K^{+}{)}^T$$不过，没有必要对每个Sigma-Point进行修正，因为根据卡尔曼增益的定义可以更快地得出结果：
$$P_k^{+}=P_k^{-}-K{R}_k{K}^T$$其中，$R_k$是测量误差的协方差（例如我们可能从位置传感器的规格表中获得的误差）。我们在此不做证明，但可以很好地解释这一结果：卡尔曼增益决定了我们可以从传播的协方差矩阵中减去多少测量噪声。回想一下，如果测量噪声很大，那么 $K$ 就会很小（测量噪声在“分母”中），因此能减去的测量噪声就很少。如果测量噪声较小，那么 $K$ 就会较大，我们就会减去较大部分的（较小的）测量噪声。结果就是，随着时间的推移，适量的 $R$ 会被去除。这意味着卡尔曼增益越大，减去的就越多，留下的协方差矩阵就越小，这反映了估算结果的确定性越高。

结果如下。请注意，$3\sigma$ 边界比我们的初始估计值要小。

图6. 修正后的状态估计值和状态估计协方差

这样就完成了西格玛点滤波器的一个循环！当有新的测量结果出现时，我们可以重新来过。前进 10 秒，结果如下：

图7. 仿真10s的结果

上下元素的不确定性在弹跳顶端是如何变得非常小的，因为我们知道球会在那里减速，而左右元素的不确定性在弹跳结束时是非常恒定的，因为向右的速度没有变化。

图8. 仿真10s的结果

到最后，尽管测量结果非常糟糕，但估计结果与事实非常吻合。事实上，这个例子中的测量结果比粒子滤波器中的测量结果还要糟糕，但滤波后Sigma-Point滤波器的准确度却明显要高得多，这是因为粒子滤波器只能粗略地表示分布情况，而Sigma-Point滤波器则非常平滑。

Sigma-Point滤波器的框架

1. 根据最新的状态估计值和协方差矩阵设置初始西格玛点。我们需要为状态的每个维度、过程噪声的每个维度、测量噪声的每个维度分别设置一个正负西格玛点，再加一个中间西格玛点；

2. 将每个Sigma-Point向前传播到新的测量时刻；

3. 为每个Sigma-Point创建预测测量值，包括传感器噪声；

4. 使用Sigma-Point的加权和作为平均预测状态和平均预测测量值；

5. 计算传播后的Sigma-Point与预测测量值之间的样本交叉协方差矩阵$P_{xy}$；

6. 计算预测测量值的样本协方差$P_{yy}$；

7. 计算卡尔曼增益$K$；

8. 修正状态估计值和协方差矩阵。

假设、优势、劣势

假设：
当不确定性的各种来源【先前的不确定性（previous uncertainty）、过程噪声（process noise）和测量噪声（measurement noise）】都是单模态（unimodal）且不相关时，Sigma-Point滤波器是一个很好的选择。

优点：

• 在满足上述假设的条件下，Sigma-Point滤波器通常比粒子过滤器更精确，因为它不依赖随机粒子。
• Sigma-Point滤波器比粒子滤波器快快快得多。粒子滤波器可能需要 1000 个点，而Sigma-Point滤波器可能只需要 9 个点左右。
• Sigma-Point滤波器的假设适用于许多不同的实际问题，建立Sigma-Point滤波器只需定义传播函数、测量函数、过程噪声协方差和测量噪声协方差，而所有这些对于粒子滤波器来说也是必要的。
• Sigma-Point滤波器有标准形式，因此在书籍或期刊中找到好的参考资料相对容易。

劣势：

• 一些问题会导致西格玛点滤波器 “崩溃”。例如，在我们的球问题中，如果时间步长较大，Sigma-Point就会在一两次反弹后变得非常 “混乱”，并可能导致样本协方差矩阵失去意义。要避免这种情况可能比较困难，而粒子滤波器就不会有这个问题。
• 尽管Sigma-Point滤波器比粒子滤波器快得多，但也比扩展卡尔曼滤波器（Extended Kalman Filter，EKF）慢得多。