自动驾驶感知融合-无迹卡尔曼滤波(Lidar&Radar)

Unscented Kalman Filter

在上一篇的文章“卡尔曼及扩展卡尔曼滤波(Lidar&Radar)”中我已经详细介绍了卡尔曼KF及扩展卡尔曼滤波EKF，今天我们来看以下在处理非线性问题上更为强大的无迹卡尔曼滤波UKF。在本文中，首先我会介绍无迹卡尔曼滤波的原理，并分析EKF和UKF的优缺点，然后我会介绍一个更符合现实场景的过程模型“恒转弯速率和速度幅值模型(CTRV)” ，最后我会在CTRV上执行UKF，从而实现基于激光雷达和毫米波雷达的多目标追踪。

Github:https://github.com/williamhyin/SFND_Unscented_Kalman_Filter

Email: williamhyin@outlook.com

知乎专栏: 自动驾驶全栈工程师

Overview

我们简单回顾下KF及EKF滤波：

• 卡尔曼滤波器: 假设数据为高斯分布形式，我们将线性方程式应用于该高斯分布的数据，然后使用一系列卡尔曼滤波公式获得最优状态预估。

• 扩展卡尔曼滤波：然而现实世界并不是线性的，当传感器在一个方向上进行测量时，我们可能会在另一个方向上进行预测，这样会涉及非线性的角度和正弦余弦函数。因此，EKF可以借助泰勒级数（以及雅可比矩阵）在高斯均值附近线性近似非线性函数，然后预测其值。

  

1. 既然EKF已经能够很好的解决非线性问题了，为什么还需要UKF？

   原因是现实生活中存在很多完全不规律的分布，我们需要更好的近似方式和更高的性能。

   让我们来看下在EKF中如何解决非线性问题的：

   

   可以看到原始高斯分布经过非线性变换之后其实是一个不规则的非线性分布，在EKF中我们在高斯均值点附近用泰勒级数近似非线性分布，从而获得近似高斯分布。但是问题在于，这个近似高斯分布有时不是足够精确，单单一个均值点往往无法预测上图中这种很不规则的分布的。这个时候我们就需要无迹卡尔曼滤波UKF了。

   值得注意的是尽管Unscented Kalman Filter通常被叫做无迹卡尔曼滤波，但是在有的文献中存在另外一种叫法：无损卡尔曼滤波，这两种叫法都是可行的。

   2. UKF是怎么解决复杂不规则分布的？

      UKF从原始高斯分布中提取包括均值点在内的一系列代表点，并将代表点带入到非线性等式中，围绕这些点进行近似，从而获得更好的近似结果。

      

      在上图中我们可以看到，蓝色椭圆代表的UKF近似高斯分布更接近真实的不规则分布。

      ​ 总的来说，与EKF相比，UKF是处理非线性过程模型或非线性测量模型的更好的替代方法。UKF不会对非线性函数进行线性化处理，它用所谓的sigma 点来近似概率分布。其优势在于在很多情况下，sigma 点近似非线性转换的效果比线性化更好，此外它还可以省去计算复杂的雅可比矩阵的过程

2. UKF 代表点的数量是不是越多越好？

   从原始高斯分布中提取的点的数目越多，我们的近似就越精确，但是相应计算资源的消耗就越大。将原始高斯分布中所有的数据点进行非线性变换和近似处理是最简单的解决方案，但不是最佳方案。

3. 如何选择合适的UKF代表点？

   UKF代表点被称为sigma 点，它们分布在状态均值的周围，并且和每个状态维度的标准偏差信号(即σ)有关，因此被叫做sigma 点。它们代表了整个分布。在这里值得注意的，我们可能会根据实际需求或多或少优先考虑某些点，从而使我们的近似性更好，这就需要为每一个sigma 点赋予权重。如何合理选择sigma 点的个数，并且为不同的sigma 点赋予权重是UKF应用的重点，我会在接下来的章节详细描述。

由于无迹卡尔曼滤波是在卡尔曼滤波的基础上开发的，在本文中对于一些基础概念我并没有详细解释，如果有什么疑问，请参看我的上一篇文章“卡尔曼及扩展卡尔曼滤波(Lidar&Radar)”。

Unscented Transform

当高斯分布经过非线性函数变换后，它不再是高斯分布，而无迹卡尔曼滤波通过无迹转换将非线性函数变换的结果近似成高斯分布。

以下是无迹变换执行的步骤：

1.计算Sigma点集

2.为每个Sigma点分配权重

3.把所有单个Sigma点代入非线性函数f

4.对经过上述加权和转换的点近似新的高斯分布

5.计算新高斯分布的均值和方差。

计算Sigma 点集

选择合适的sigma 点数量n取决于系统的状态维度N：n=2N+1。

​ 公式１

我们可以根据公式1计算sigma 点 矩阵，值得注意的是到第１个点和其之后的点有不同的计算公式，这是由于第１个点是均值点，其余点都是围绕均值的sigma 点。

以下是公式１中部分参数的含义：

• χ表示 sigma 点矩阵。矩阵中每一列都表示一组sigma 点。如果系统是2维空间，那么矩阵的大小将是2 x 5。 每个维度有n=2N+1＝5个sigma 点。可以看到，同一列的sigma 点有两个对称的计算公式，这是由于同一列的上下两个sigma 点在原始高斯分布中围绕均值点呈对称分布。
• λ是比例因子，它告诉我们应该选择距离均值多远的sigma 点。 数学研究表明，λ的最佳值为3-n。
• Σ被称为协方差矩阵，对Σ求平方需要找到一个满足以下条件的矩阵 S:　Σ = S.S or Σ = S.S_transpose，S= √∑。

计算sigma点的权重

在我们挑选出合适的sigma 点之后，我们需要为每个sigma 点赋予合适的权重:

​ 公式２

值得注意的是计算均值权重的公式与计算其余sigma 点权重的公式也是不同的，而且所有代表点权重之和等于1。

无迹转换参数

sigma 点的选择和权重计算公式主要有以下四个可调节参数，这些参数被称作UT Parameters, 我们可以使用这些参数对无迹变换进行缩放：

其中最重要的是 λ 扩散参数，数学研究表明，λ的最佳值为3-n。如果λ已经给出，其余UT参数可以考虑省略，从而简化公式，如在有的资料中计算sigma 点权重的公式为

如在这个公式中没有ωc和ωm。

本项目中使用简化的sigma 点权重计算公式。

但是理解 λ 扩散参数的原理是很有必要的， λ 扩散参数由k，α，n三个参数计算得到，下图是不同k和α的组合对于sigma 点分布的影响。