自动驾驶定位系统-无迹卡尔曼滤波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利用概率分布逼近来解决非线性函数逼近的问题。

以一维的高斯分布为例,如下图所示，左侧是一维高斯分布，$h(x)$是非线性变化，右侧是变换后的高斯分布。
图片来源:State Estimation and Localization for Self-Driving Cars：Lesson 6: An Alternative to the EKF - The Unscented Kalman Filter

左侧的高斯概率分布参数是已知的，$x \sim N(\mu,\sigma)$；非线性变换$y=h(x)$也是已知；如何估计非线性变换后的分布呢？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分解。

$$ LL^T = \Sigma $$

2）计算Sigma Point。

$$ x_0 = \mu $$

$$ x_i = \mu + \sqrt{N+K} \text{col}_i L \ \ \ i=1,2,...,N $$

$$ x_{N-i} = \mu - \sqrt{N+K} \text{col}_i L \ \ \ i=1,2,...,N $$

其中N是高斯分布的维度，K是可调参数，通常设置$K=3-N$是一个好的选择。

2.2 Transform Sigma Points

将Sigma Points通过非线性变换$h(x)$映射到Output Distribution。

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

将$2N+1$个Sigma Point代入Nonlinear Function $y=h(x)$

$$ y_i = h(x_i) \ \ \ i = 0,1,...,2N $$

2.3 Compute Weighted Mean And Covariance of Transformed Sigma Points。

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

$$ \mu_y = \sum_{i=0}^{2N} \alpha_i y_i $$

$$ \Sigma_{yy} = \sum_{i=0}^{2N} \alpha_i (y_i - \mu_y)(y_i - \mu_y)^T $$

其中:

$$ \alpha_i = \begin{cases} \frac{K}{N+K} & i=0 \\ \frac{1}{2(N+K)}& otherwise \end{cases} $$

3.The Unscented Kalman Filter (UKF)

3.1 None Linear Motion Model:

$$ x_k = f(x_{k-1}, u_k, w_k) $$

$$ w_k \sim N(0, Q_k) $$

3.2 None Linear Measurement Model:

$$ y_k = h(x_k, v_k) $$

$$ v_k \sim N(0, R_k) $$

3.3 Prediction Steps

1）Compute Sigma Points:

$$ \hat{L}_{k-1} \hat{L}_{k-1} = \hat{P}_{k-1} $$

$$ \hat{x}^{(0)}_{k-1} = \hat{x}_{k-1} $$

$$ \hat{x}^{(i)}_{k-1} = \hat{x}_{k-1} + \sqrt{N + K} \text{col}_i \hat {L}_{k-1} \ \ \ i = 1,2,...,N $$

$$ \hat{x}^{(i + N)}_{k-1} = \hat{x}_{k-1} + \sqrt{N + K} \text{col}_i \hat {L}_{k-1} \ \ \ i = 1,2,...,N $$

2）Propagate Sigma Points:

$$ \check{x}_k^{(i)} = f(\check{x}_{k-1}^{(i)}, u_k, 0) \ \ \ i = 1, 2,..., 2N $$

3）Compute Predicted Mean And Covariance

$$ \alpha^{(i)} = \begin{cases} \frac{K}{N+K} & i=0 \\ \frac{1}{2} \frac{1}{N+K}& otherwise \end{cases} $$

$$ \check{x}_k = \sum_{i=0}^{2N} \alpha^{(i)} \check{x}_k^{(i)} $$

$$ \check{P}_k=\sum_{i=0}^{2N} \alpha^{(i)} (\check{x}_k^{(i)} - \check{x}_k) (\check{x}_k^{(i)} - \check{x}_k)^T + Q_k $$

3.4 Correction Steps

1） Predict Measurement From Propagated Sigma Points

$$ \hat{y}_k^{(i)} = h(\check{x}_k^{(i)}, 0) \ \ \ i=1,2,...,2N $$

2） Estimate Mean And Covariance of Predicted Measurement

$$ \hat{y}_k = \sum_{i=0}^{2N} \alpha^{(i)} \hat{y}_k^{(i)} $$

$$ {P}_y=\sum_{i=0}^{2N} \alpha^{(i)} (\hat{y}_k^{(i)} - \hat{y}_k) (\hat{y}_k^{(i)} - \hat{y}_k)^T + R_k $$

3） Compute Cross-Covariance And Kalman Gain

$$ P_{xy} = \sum_{i=0}^{2N} \alpha^{(i)} (\check{x}_k^{(i)} - \check{x}_k) (\hat{y}_k^{(i)} - \hat{y}_k)^T $$

$$ K_k=P_{xy} P_y^{-1} $$

4）Compute Corrected Covariance And Mean

$$ \hat{x}_k = \check{x}_k + K_k ({y}_k - \hat{y}_k) $$

$$ \hat{P}_k = \check{P}_k - K_k P_y K_k^T $$

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

4.1 已知参数

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

$$ \begin{aligned} \mathbf{\hat{x}}_{k-1} &= \begin{bmatrix} \text{p}\\ \text{v} \end{bmatrix}\\ \end{aligned} $$

车辆加速度$u=a$，LandMark的位置参数$(S,D)$均为已知。

车辆的Motion Model：

$$ \begin{aligned} \check{x}_k =& f(\hat{x}_{k-1}, u_k, w_k) \\ =& \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \\ \end{bmatrix} x_{k-1} + \begin{bmatrix} 0 \\ \Delta t \\ \end{bmatrix} u_k + w_k \end{aligned} $$

车辆的Measurement Model:

$$ \begin{aligned} y_k =& h(p_k, v_k) \\ =& tan^{-1} \bigg ( \frac{S}{D-p_k} \bigg ) + v_k \\ \end{aligned} $$

$$ v_k \sim N(0, 0.01) $$

$$ w_k \sim N \bigg (\begin{bmatrix} 0 \\ 0 \\ \end{bmatrix}, \begin{bmatrix} 0.1 & 0.0 \\ 0.0 & 0.1 \\ \end{bmatrix} \bigg) $$

系统的初始值:

$$ \hat{x}_0 \sim N \bigg (\begin{bmatrix} 0 \\ 5 \\ \end{bmatrix}, \begin{bmatrix} 0.01 & 0.0 \\ 0.0 & 1.0 \\ \end{bmatrix} \bigg) $$

$\Delta t=0.5s$，$u_0=-2m/s^2$，$y_1=\pi / 6$ rad，$S=20m$，$D=40m$。

4.2 应用UKF

首先是Prediction过程：

$$N=2,K=3-N=1$$

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

$$ \begin{aligned} \hat{L}_0 \hat{L}_0 &= \hat{P}_0 \\ &= \begin{bmatrix} 0.01 & 0.0 \\ 0.0 & 1.0 \\ \end{bmatrix}\\ \end{aligned} $$

可得:

$$ L_0 = \begin{bmatrix} 0.1 & 0.0 \\ 0.0 & 1.0 \\ \end{bmatrix} $$

计算Sigma Points：

$$ \hat{x}_0^{(0)} = \begin{bmatrix} 0 \\ 5 \\ \end{bmatrix} $$

$$ \hat{x}_0^{(1)} = \begin{bmatrix} 0 \\ 5 \\ \end{bmatrix}+ \sqrt{3} \begin{bmatrix} 0.1 \\ 0 \\ \end{bmatrix} = \begin{bmatrix} 0.2 \\ 5 \\ \end{bmatrix} $$

$$ \hat{x}_0^{(2)} = \begin{bmatrix} 0 \\ 5 \\ \end{bmatrix}+\sqrt{3} \begin{bmatrix} 0 \\ 0.1 \\ \end{bmatrix} = \begin{bmatrix} 0 \\ 6.7 \\ \end{bmatrix} $$

$$ \hat{x}_0^{(3)} = \begin{bmatrix} 0 \\ 5 \\ \end{bmatrix}-\sqrt{3} \begin{bmatrix} 0.1 \\ 0 \\ \end{bmatrix} = \begin{bmatrix} -0.2 \\ 5 \\ \end{bmatrix} $$

$$ \hat{x}_0^{(4)} = \begin{bmatrix} 0 \\ 5 \\ \end{bmatrix}-\sqrt{3} \begin{bmatrix} 0 \\ 0.1 \\ \end{bmatrix} = \begin{bmatrix} 0 \\ 3.3 \\ \end{bmatrix} $$

对Sigma Points执行Transforming过程：

$$ \alpha^{(i)}=\left\{\begin{array}{ll}{\frac{\kappa}{N+\kappa}=\frac{1}{2+1}=\frac{1}{3}} & {i=0} \\ {\frac{1}{2} \frac{1}{N+\kappa}=\frac{1}{2} \frac{1}{2+1}=\frac{1}{6}} & {\text { otherwise }}\end{array}\right. $$

$$ \begin{array}{l}{\check{\mathbf{x}}_{1}^{(0)}=\left[\begin{array}{ll}{1} & {0.5} \\ {0} & {1}\end{array}\right]\left[\begin{array}{l}{0} \\ {5}\end{array}\right]+\left[\begin{array}{l}{0} \\ {0.5}\end{array}\right](-2)=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]} \\ {\check{\mathbf{x}}_{1}^{(1)}=\left[\begin{array}{ll}{1} & {0.5} \\ {0} & {1}\end{array}\right]\left[\begin{array}{l}{0.2} \\ {5}\end{array}\right]+\left[\begin{array}{c}{0} \\ {0.5}\end{array}\right](-2)=\left[\begin{array}{c}{2.7} \\ {4}\end{array}\right]} \\ {\check{\mathbf{x}}_{1}^{(2)}=\left[\begin{array}{cc}{1} & {0.5} \\ {0} & {1}\end{array}\right]\left[\begin{array}{c}{0} \\ {6.7}\end{array}\right]+\left[\begin{array}{c}{0} \\ {0.5}\end{array}\right](-2)=\left[\begin{array}{c}{3.4} \\ {5.7}\end{array}\right]} \\ {\check{\mathbf{x}}_{1}^{(3)}=\left[\begin{array}{cc}{1} & {0.5} \\ {0} & {1}\end{array}\right]\left[\begin{array}{c}{-0.2} \\ {5}\end{array}\right]+\left[\begin{array}{c}{0 .} \\ {0.5}\end{array}\right](-2)=\left[\begin{array}{c}{2.3} \\ {4}\end{array}\right]} \\ {\check{\mathbf{x}}_{1}^{(4)}=\left[\begin{array}{cc}{1} & {0.5} \\ {0} & {1}\end{array}\right]\left[\begin{array}{c}{0} \\ {3.3}\end{array}\right]+\left[\begin{array}{c}{0} \\ {0.5}\end{array}\right](-2)=\left[\begin{array}{c}{1.6} \\ {2.3}\end{array}\right]}\end{array} $$

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

$$ \begin{aligned} \check{\mathbf{x}}_{k} &=\sum_{i=0}^{2 N} \alpha^{(i)} \check{\mathbf{x}}_{k}^{(i)} \\ &=\frac{1}{3}[2.5]+\frac{1}{6}\left[\begin{array}{c}{2.7} \\ {4}\end{array}\right]+\frac{1}{6}\left[\begin{array}{c}{3.4} \\ {5.7}\end{array}\right]+\frac{1}{6}\left[\begin{array}{c}{2.3} \\ {4}\end{array}\right]+\frac{1}{6}\left[\begin{array}{c}{1.6} \\ {2.3}\end{array}\right]=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right] \end{aligned} $$

$$ \begin{aligned} \check{\mathbf{P}}_{k} &=\sum_{i=0}^{2 N} \alpha^{(i)}\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)^{T}+\mathbf{Q}_{k-1} \\ &=\frac{1}{3}\left(\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)\left(\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)^{T} \\ &+ \frac{1}{6}\left(\left[\begin{array}{c}{2.7} \\ {4}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)\left(\left[\begin{array}{c}{2.7} \\ {4}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)^{T} \\ &+\frac{1}{6}\left(\left[\begin{array}{c}{3.4} \\ {5.7}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)\left(\left[\begin{array}{c}{3.4} \\ {5.7}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)^{T} \\ &+ \frac{1}{6}\left(\left[\begin{array}{c}{2.3} \\ {4}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)\left(\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)^{T} \\ &+\frac{1}{6}\left(\left[\begin{array}{c}{1.6} \\ {2.3}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)\left(\left[\begin{array}{c}{1.6} \\ {2.3}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)^{T}+\left[\begin{array}{cc}{0.1} & {0} \\ {0} & {0.1}\end{array}\right] \\&=\left[\begin{array}{cc}{0.36} & {0.5} \\ {0.5} & {1.1}\end{array}\right] \end{aligned} $$

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

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

$$ \begin{array}{l}{\check{\mathbf{L}}_{k} \check{{\mathbf{L}}}_{k}^{T}=\check{\mathbf{P}}_{k}} \\ {\left[\begin{array}{cc}{0.60} & {0} \\ {0.83} & {0.64}\end{array}\right]\left[\begin{array}{cc}{0.60} & {0} \\ {0.83} & {0.64}\end{array}\right]^{T}=\left[\begin{array}{cc}{0.36} & {0.5} \\ {0.5} & {1.1}\end{array}\right]}\end{array} $$

Sigma Points采样：

$$ \begin{array}{l}\check{\mathbf{x}}_{1}^{(0)}=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\\{\check{\mathbf{x}}_{1}^{(1)}=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]+\sqrt{3}\left[\begin{array}{c}{0.60} \\ {0.83}\end{array}\right]=\left[\begin{array}{c}{3.54} \\ {5.44}\end{array}\right]} \\ {\check{\mathbf{x}}_{1}^{(2)}=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]+\sqrt{3}\left[\begin{array}{c}{0} \\ {0.64}\end{array}\right]=\left[\begin{array}{c}{2.5} \\ {5.10}\end{array}\right]} \\ {\check{\mathbf{x}}_{1}^{(3)}=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]-\sqrt{3}\left[\begin{array}{c}{0.60} \\ {0.83}\end{array}\right]=\left[\begin{array}{c}{1.46} \\ {2.56} \\ \end{array}\right]} \\ \check{\mathbf{x}}_{1}^{(4)}=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]-\sqrt{3}\left[\begin{array}{c}{0} \\ {0.64}\end{array}\right]=\left[\begin{array}{c}{2.5} \\ {2.90}\end{array}\right] \end{array} $$

执行Transforming变换：

$$ \hat{y}_{1}^{(i)}=h_{1}\left(\check{\mathbf{x}}_{1}^{(i)}, 0\right) \quad i=0, \ldots, 2 N $$

$$ \begin{array}{l}{\hat{y}_{1}^{(0)}=\tan ^{-1}\left(\frac{20}{40-2.5}\right)=0.49} \\ {\hat{y}_{1}^{(1)}=\tan ^{-1}\left(\frac{20}{40-3.54}\right)=0.50} \\ {\hat{y}_{1}^{(2)}=\tan ^{-1}\left(\frac{20}{40-2.5}\right)=0.49} \\ {\hat{y}_{1}^{(3)}=\tan ^{-1}\left(\frac{20}{40-1.46}\right)=0.48} \\ \hat{y}_{1}^{(4)}=\tan ^{-1}\left(\frac{20}{40-2.5}\right)=0.49 \end{array} $$

计算均值和协方差：

$$ \begin{aligned} \hat{y}_{1} &=\sum_{i=0}^{2 N} \alpha^{(i)} \hat{y}_{1}^{(i)} \\ &=\frac{1}{3}(0.49)+\frac{1}{6}(0.50)+\frac{1}{6}(0.49)+\frac{1}{6}(0.48)+\frac{1}{6}(0.49) \\ &=0.49 \end{aligned} $$

$$ \begin{aligned} P_{y} &=\sum_{i=0}^{2 N} \alpha^{(i)}\left(\hat{y}_{1}^{(i)}-\hat{y}_{k}\right)\left(\hat{y}_{1}^{(i)}-\hat{y}_{k}\right)^{T}+R_{k} \\ &=\frac{1}{3}(0.49-0.49)^{2}+\frac{1}{6}(0.50-0.49)^{2}+\frac{1}{6}(0.49-0.49)^{2}+\frac{1}{6}(0.48-0.49)^{2}+\frac{1}{6}(0.49-0.49)^{2}+0.01 \\ &=0.01 \end{aligned} $$

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

$$ \begin{aligned} \mathbf{P}_{x y} &=\sum_{i=0}^{2 N} \alpha^{(i)}\left(\check{\mathbf{x}}_{k}^{(i)}-\check{\mathbf{x}}_{k}\right)\left(\hat{y}_{1}^{(i)}-\hat{y}_{k}\right)^{T} \\ &=\frac{1}{3}\left(\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)(0.49-0.49) \\ &+ \frac{1}{6}\left(\left[\begin{array}{c}{3.54} \\ {5.44}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)(0.50-0.49) \\ & +\frac{1}{6}\left(\left[\begin{array}{c}{2.5} \\ {5.10}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)(0.49-0.49) \\ &+\frac{1}{6}\left(\left[\begin{array}{c}{1.46} \\ {2.56}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)(0.48-0.49) \\ &+\frac{1}{6}\left(\left[\begin{array}{c}{2.5} \\ {2.90}\end{array}\right]-\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]\right)(0.49-0.49) \\ &=\left[\begin{array}{l}{0.004} \\ {0.006}\end{array}\right] \end{aligned} $$

$$ \mathbf{K}_{1}=\mathbf{P}_{x y} P_{y}^{-1}=\left[\begin{array}{l}{0.004} \\ {0.006}\end{array}\right] \frac{1}{0.01}=\left[\begin{array}{l}{0.40} \\ {0.55}\end{array}\right] $$

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

$$ \hat{\mathbf{x}}_{1}=\check{\mathbf{x}}_{1}+\mathbf{K}_{1}\left(y_{1}-\hat{y}_{1}\right)=\left[\begin{array}{c}{2.5} \\ {4}\end{array}\right]+\left[\begin{array}{c}{0.40} \\ {0.55}\end{array}\right](0.52-0.49)=\left[\begin{array}{c}{2.51} \\ {4.02}\end{array}\right] $$