自動駕駛定位系統-無跡卡爾曼濾波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)$是非線性變化，右側是變換後的高斯分佈。

左側的高斯機率分佈參數是已知的，$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。

Sigma Points個數

Sigma Point的個數如何選擇呢？一般狀況下，N維的高斯分佈選擇2N+1個Sigma Point(一個Point是Mean，其它Point關於Mean對稱分佈)。一維高斯分佈選擇3個Sigma Point，二維高斯分佈選擇5個Sigma Point。

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。

將$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] $$

參考連接

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/...

相關文章

自動駕駛定位系統-Error State Extend Kalman Filter

自動駕駛定位系統-擴展卡爾曼濾波Extend Kalman Filter

自動駕駛定位系統-卡爾曼濾波Kalman Filter

自動駕駛系統定位與狀態估計- Recursive Least Squares Estimation

自動駕駛系統定位與狀態估計- Weighted Least Square Method

自動駕駛定位系統-State Estimation & Localization

自動駕駛定位算法-直方圖濾波定位

---

我的網站地址: http://www.banbeichadexiaojiu...