从零开始学习自动驾驶系统(七)-无迹卡尔曼滤波Unscented Kalman Filter
从零开始学习自动驾驶系统(七)-无迹卡尔曼滤波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