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

時間 2020-02-16

標籤自動駕駛定位系統擴展卡爾濾波 extend kalman filter 欄目系統網絡简体版

原文原文鏈接

1.泰勒級數展開

若是函數$f(x)$在點$x=x_0$具備任意階導數,則算法

$$ \begin{aligned} f(x)=&\sum_{n=0}^{\infty} \frac{f^{(n)}(x_0)}{n!} (x - x_0)^n \\ =& f(x_0) + f(x_0)^{\prime}(x-x_0) + \frac{f^{\prime \prime}(x_0)}{2!}(x-x_0)^2 + ... + \frac{f^{(n)}(x_0)}{n!} (x - x_0)^n \end{aligned} $$segmentfault

稱爲$f(x)$在點$x_0$處的泰勒級數。咱們可使用泰勒級數來逼近非線性函數。app

以$cos(x)$爲例，它的泰勒級數以下:函數

$$ cos(x)|_{x=x_0} = cos(x_0) - sin(x_0)(x-x_0) - \frac{cos(x_0)}{2!}(x-x_0)^2 + ... $$網站

假定在$x=0$處展開:spa

$$ \begin{aligned} cos(x)|_{x=0} =& cos(0) - sin(0)x - \frac{cos(0)}{2!}x^2 + ... \\ =& 1 - \frac{x^2}{2} + ... \end{aligned} $$翻譯

$1 - \frac{x^2}{2}$稱爲$cos(x)$在$x=0$處的二階泰勒展開。能夠看到距離$x=0$越近，兩者的差異就越小，隨着距離的增長，偏差不斷增長。3d

x	cos(x)	$1 - \frac{x^2}{2}$
0	1	1
0.25	0.969	0.969
0.5	0.878	0.875
0.75	0.732	0.719
1.0	0.540	0.500
1.25	0.315	0.219
1.50	0.071	-0.125

非線性函數線性化是將nonlinear function在展開點$x=x_0$附近進行一階泰勒展開。blog

$$ f(x) = f(x_0) + f^{\prime}(x_0)(x-x_0) $$圖片

下圖是sin(x)的函數圖像以及在x=0處的一階泰勒的函數圖像，能夠看到，在x=0附近，兩者很是接近，一階泰勒展開能夠很好的逼近sin(x)。

2. 線性卡爾曼濾波 (linearized Kalman filter)

線性卡爾曼濾波經過一階泰勒級數將非線性系統(nonlinear system)線性化，從而知足標準卡爾曼濾波對於線性化的要求。

2.1 非線性系統模型

假設非線性系統的模型以下:

Nonlinear Motion/Process Model：

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

其中$x_k$是Current State，$x_{k-1}$是Previous State，$u_k$是Input，$w_k$是Process Noise.

Nonlinear Measurement Model:

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

$y_k$是Measurement，$x_k$是State，$v_k$是Measurement Noise。

Process Noise和Measurement Noise均服從0均值的正態分佈：

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

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

其中函數$f(.)$和h(.)都是非線性函數。

2.2 線性展開

Nonlinear Motion Model展開:

$$ \begin{aligned} x_{k} =& f(x_{k-1}, u_k, w_k) \\ \approx & f(\hat{x}_{k-1}, u_k, 0) + \underbrace{ \frac{\partial f}{\partial x_{k-1}} \bigg |_{(\hat{x}_{k-1}, u_k, 0)} (x_{k-1} - \hat{x}_{k-1}) }_{F_k} + \underbrace{\frac{\partial f}{\partial w_{k}} \bigg |_{(\hat{x}_{k-1}, u_k, 0)} w_{k} }_{L_k} \\ \end{aligned} $$

Nonlinear Measurement Model展開:

$$ \begin{aligned} y_k =& h(x_k, v_k) \\ \approx & h( \check{x}_k, 0) + \underbrace{\frac{\partial h}{\partial x_k} \bigg |_{(\check{x}_k,0)} (x_k - \check{x}_k)}_{H_k} + \underbrace{\frac{\partial h}{\partial v_k} \bigg |_{(\check{x}_k,0)} v_k}_{M_k} \end{aligned} $$

其中$F_k$、$L_k$、$H_K$、$M_k$被稱爲Jacobian Matrix。

因而在展開點附近，非線性系統就能夠當作線性系統來處理.

$$ \begin{aligned} x_{k} =& f(\hat{x}_{k-1}, u_k, 0) + F_k(x_{k-1} - \hat{x}_{k-1}) + L_kw_{k} \\ \end{aligned} $$

$$ \begin{aligned} y_{k} = & h( \check{x}_k, 0) + H_k (x_k - \check{x}_k) + M_k v_k \end{aligned} $$

根據標準卡爾曼濾波理論:

Predection

$$ \check{x}_k = f(\hat{x}_{k-1}, u_k, 0) $$

$$ \check{P}_k = F_k \hat{P}_{k-1} F_k^T + L_k Q_k L_k^T $$

Measurement Update

$$ K_k = \check{P}_k H_k^T(H_k \check{P}_kH_k^T + M_k R_k M_k^T)^{-1} $$

$$ \hat{x}_k = K_k (y_k - h_k(\check{x}_k,0)) $$

$$ \hat{P}_k = (1 - K_k H_k) \check{P}_k $$

EKF定位系統

以車輛定位爲例，以下圖所示，已知LandMark的位置參數$(S,D)$，車輛加速度$u=a$，車輛安裝了角度測量的設備，可以測量與LandMark的視角。

已知車輛在k-1時刻的State的矩陣形式以下:

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

車輛的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) $$

計算Jacobian Matrix:$F_k$,$L_k$,$H_k$,$M_k$。

$$ F_k = \frac{\partial f}{\partial x_{k-1}} \bigg |_{(\hat{x}_{k-1}, u_k, 0)} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \\ \end{bmatrix} $$

$$ L_k=\frac{\partial f}{\partial w_{k}} \bigg |_{(\hat{x}_{k-1}, u_k, 0)} = I_{2 \times 2} $$

$$ H_k = \frac{\partial h}{\partial x_k} \bigg |_{(\check{x}_k,0)} = \begin{bmatrix} \frac{(D-p_k)^2}{S^2 + (D-p_k)^2} * \frac{-S}{(D-p_k)^2} * (-1) & 0 \end{bmatrix} = \begin{bmatrix} \frac{S}{S^2 + (D-p_k)^2} & 0 \end{bmatrix} $$

$$ M_K = \frac{\partial h}{\partial v_k} \bigg |_{(\check{x}_k,0)} = 1 $$

給定系統的初始值:

$$ \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，LandMark的位置信息$S=20m$，$D=40m$。

把上述信息代入Extend Kalman Filter:

$$ \begin{aligned} \check{x}_1 = &f(\hat{x}_0, u_1, 0) \\ =& \begin{bmatrix} 1 & 0.5 \\ 0 & 1 \\ \end{bmatrix} \begin{bmatrix} 0 \\ 5 \\ \end{bmatrix} + \begin{bmatrix} 0 \\ 0.5 \\ \end{bmatrix} (-2.0)\\ =& \begin{bmatrix} 2.5 \\ 4.0 \\ \end{bmatrix} \end{aligned} $$

$$ \begin{aligned} \check{P}_1 =& F_1 \hat{P}_0 F_1^T + L_1 Q_1 L_1^T \\ =& \begin{bmatrix} 1 & 0.5 \\ 0 & 1 \\ \end{bmatrix} \begin{bmatrix} 0.01 & 0.0 \\ 0.0 & 1.0 \\ \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0.5 & 1 \\ \end{bmatrix} + \begin{bmatrix} 1 & 0 \\ 0 & 1 \\ \end{bmatrix} \begin{bmatrix} 0.1 & 0.0 \\ 0.0 & 0.1 \\ \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 1 \\ \end{bmatrix}\\ =& \begin{bmatrix} 0.36 & 0.5 \\ 0.5 & 1.1 \\ \end{bmatrix} \end{aligned} $$

$$ \begin{aligned} K_1 =& \check{P}_1 H_1^T(H_1 \check{P}_1 H_1^T + M_1 R_1 {M_1^T})^{-1} \\ =& \begin{bmatrix} 0.36 & 0.5 \\ 0.5 & 1.1 \\ \end{bmatrix} \begin{bmatrix} 0.011 \\ 0.0 \\ \end{bmatrix} \bigg ( \begin{bmatrix} 0.011 & 0.0 \end{bmatrix} \begin{bmatrix} 0.36 & 0.5 \\ 0.5 & 1.1 \\ \end{bmatrix} \begin{bmatrix} 0.011 \\ 0.0 \\ \end{bmatrix} + 1(0.01)(1) \bigg )^{-1} \\ =& \begin{bmatrix} 0.40 \\ 0.55 \\ \end{bmatrix} \end{aligned} $$

$$ \begin{aligned} \hat{x}_1 =& K_1 (y_1 - h_1(\check{x}_1,0)) \\ =& \begin{bmatrix} 0.40 \\ 0.55 \\ \end{bmatrix} \bigg ( \frac{\pi}{6.0} - 0.49 \bigg ) \\ =& \begin{bmatrix} 2.51 \\ 4.02 \\ \end{bmatrix} \\ \end{aligned} $$

$$ \begin{aligned} \hat{P}_1 =& (1 - K_1 H_1) \check{P}_1 \\ =& \begin{bmatrix} 0.36 & 0.5 \\ 0.5 & 1.1 \\ \end{bmatrix} \\ \end{aligned} $$

能夠看出，Motion Model給出的車輛位置爲2.50,速度爲4.0m/s;Measurement Model給出的車輛位置爲2.51,速度爲4.02m/s。