自动驾驶定位系统-扩展卡尔曼滤波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} $$

称为$f(x)$在点$x_0$处的泰勒级数。我们可以使用泰勒级数来逼近非线性函数。

以$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$处展开:

$$ \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$越近，二者的差别就越小，随着距离的增加，误差不断增加。

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$附近进行一阶泰勒展开。

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

下图是sin(x)的函数图像以及在x=0处的一阶泰勒的函数图像，可以看到，在x=0附近，二者非常接近，一阶泰勒展开可以很好的逼近sin(x)。
图片来源:https://www.embedded.com/design/connectivity/4025693/Using-nonlinear-Kalman-filtering-to-estimate-signals

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的视角。

图片来源: Coursera:State Estimation - Linear and Nonlinear Kalman Filters

已知车辆在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。