从零开始学习自动驾驶系统(七)-无迹卡尔曼滤波Unscented Kalman Filter

Unscented Kalman Filter是解决非线性卡尔曼滤波的另一种思路，它利用Unscented Transform来解决概率分布非线性变换的问题。UnScented Kalman Filter不需要像Extended Kalman Filter一样计算Jacobin矩阵，在计算量大致相当的情况下，能够获得更加精确非线性处理效果。

1.Unscented Kalman Filter的思想

it is easier to approximate a probability distribution than it is approximate an arbitary nonlinear function.

逼近概率分布要比逼近任意的非线性函数要容易的多，基于这种思想，Unscented Kalman Filter利用概率分布逼近来解决非线性函数逼近的问题。

以一维的高斯分布为例,如下图所示，左侧是一维高斯分布，

是非线性变化，右侧是变换后的高斯分布。

左侧的高斯概率分布参数是已知的

；非线性变换

也是已知；如何估计非线性变换后的分布呢？Unscented Transform提供了这样一种对变换后的概率分布的估计方法。

2.Unscented Transform

Unscented Transform的流程如下:

2.1 Choose Sigma Points.

首先从Input Distribution进行点采样，注意，这里不是随机采样，采样点距离Input Distribution的mean距离是标准差的倍数，因此这些采样点也被称为Sigma Point。Unscented Transform有时也被称为Sigma Point Transform。

图片来源:State Estimation and Localization for Self-Driving Cars：Lesson 6: An Alternative to the EKF - The Unscented Kalman Filter

Sigma Points个数

Sigma Point的个数如何选择呢？通常情况下，N维的高斯分布选择2N+1个Sigma Point(一个Point是Mean，其它Point关于Mean对称分布)。一维高斯分布选择3个Sigma Point，二维高斯分布选择5个Sigma Point。

图片来源:State Estimation and Localization for Self-Driving Cars：Lesson 6: An Alternative to the EKF - The Unscented Kalman Filter

Sigma Points的选取

1） 计算协方差矩阵的Cholesky分解。

2） 计算Sigma Point。

其中N是高斯分布的维度，K是可调参数，通常设置

是一个好的选择。

2.2 Transform Sigma Points

将Sigma Points通过非线性变换

映射到Output Distribution。

图片来源:State Estimation and Localization for Self-Driving Cars：Lesson 6: An Alternative to the EKF - The Unscented Kalman Filter

将

个Sigma Point代入Nonlinear Function

2.3 Compute Weighted Mean And Covariance of Transformed Sigma Points。

通过Sigma Points的映射点计算Output Distribution的均值和方差，从而实现对Output Distribution的分布估计。

其中:

3.The Unscented Kalman Filter (UKF)

3.1 None Linear Motion Model:

3.2 None Linear Measurement Model:

3.3 Prediction Steps

1）Compute Sigma Points:

2）Propagate Sigma Points:

3）Compute Predicted Mean And Covariance

3.4 Correction Steps

1） Predict Measurement From Propagated Sigma Points

2） Estimate Mean And Covariance of Predicted Measurement

3） Compute Cross-Covariance And Kalman Gain

4）Compute Corrected Covariance And Mean

4.UKF在自动驾驶定位中的应用举例

ukf_loc_sample.png

4.1 已知参数

已知机器人在k-1时刻的State的矩阵形式如下:

车辆加速度

，LandMark的位置参数

均为已知。

车辆的Motion Model：

车辆的Measurement Model:

系统的初始值:

4.2 应用UKF

首先是Prediction过程：

计算协方差矩阵的Cholesky分解：

可得:

计算Sigma Points：

对Sigma Points执行Transforming过程：

计算Transforming之后的Sigma Points的均值和方差：

其次是Correction的过程，与Prediction过程类似。

计算协方差矩阵的Cholesky分解：

Sigma Points采样：

执行Transforming变换：

计算均值和协方差：

计算交叉协方差和卡尔曼增益：

获得t=1时刻的车辆状态：

参考链接

1）本文主要来自Coursera自动驾驶课程: State Estimation and Localization for Self-Driving Cars：Lesson 6: An Alternative to the EKF - The Unscented Kalman Filter

2）Research Paper: https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf

个人网站地址: http://www.banbeichadexiaojiubei.com

自动驾驶学习系列：

从零开始学习自动驾驶系统-State Estimation & Localization(一)

从零开始学习自动驾驶系统-State Estimation & Localization(二)

从零开始学习自动驾驶系统(三)-State Estimation & Localization

从零开始学习自动驾驶系统(四)-卡尔曼滤波Kalman Filter

从零开始学习自动驾驶系统(五)-扩展卡尔曼滤波Extend Kalman Filter

从零开始学习自动驾驶系统(六)-Error State卡尔曼滤波