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利用概率分布逼近来解决非线性函数逼近的问题。
以一维的高斯分布为例,如下图所示,左侧是一维高斯分布,
左侧的高斯概率分布参数是已知的,
2.Unscented Transform
Unscented Transform的流程如下:
2.1 Choose Sigma Points
首先从Input Distribution进行点采样,注意,这里不是随机采样,采样点距离Input Distribution的mean距离是标准差的倍数,因此这些采样点也被称为Sigma Point。Unscented Transform有时也被称为Sigma Point Transform。
Sigma Points个数
Sigma Point的个数如何选择呢?通常情况下,N维的高斯分布选择2N+1个Sigma Point(一个Point是Mean,其它Point关于Mean对称分布)。一维高斯分布选择3个Sigma Point,二维高斯分布选择5个Sigma Point。
Sigma Points的选取
1) 计算协方差矩阵的Cholesky分解。
2) 计算Sigma Point。
其中N是高斯分布的维度,K是可调参数,通常设置
2.2 Transform Sigma Points
将Sigma Points通过非线性变换$h(x)$映射到Output Distribution。
将
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在自动驾驶定位中的应用举例
4.1 已知参数
已知机器人在k-1时刻的State的矩阵形式如下:
车辆加速度
车辆的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
自动驾驶学习系列:
自动驾驶State Estimation And Localization(六)-ES-EKF
自动驾驶State Estimation And Localization(五)-扩展卡尔曼滤波
自动驾驶State Estimation And Localization(四)-卡尔曼滤波