扩展卡尔曼滤波EKF与多传感器融合

图片名称

Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍EKF,并介绍其在无人驾驶多传感器融合上的应用。

KF与EKF

本文假定读者已熟悉KF,若不熟悉请参考卡尔曼滤波简介

KF与EKF的区别如下:

  1. 预测未来:$x’=Fx+u$用$x’=f(x,u)$代替;其余$F$用$F_j$代替。
  2. 修正当下:将状态映射到测量的$Hx’$用$h(x’)$代替;其余$H$用$H_j$代替。

其中,非线性函数$f(x,u),h(x’)$用非线性得到了更精准的状态预测值、映射后的测量值;线性变换$F_j,H_j$通过线性变换使得变换后的$x,z$仍满足高斯分布的假设。

$F_j,H_j$计算方式如下:

$$\begin{split}
F_j &= \frac{\partial f(x, u)}{\partial x} \\
b &= \frac{\partial h(x’)}{\partial x}
\end{split}$$

这里写图片描述

为什么要用EKF

KF的假设之一就是高斯分布的$x$预测后仍服从高斯分布,高斯分布的$x$变换到测量空间后仍服从高斯分布。可是,假如$F、H$是非线性变换,那么上述条件则不成立。

将非线性系统线性化

既然非线性系统不行,那么很自然的解决思路就是将非线性系统线性化。

对于一维系统,采用泰勒一阶展开即可得到:

$$f(x) \approx f(\mu) + \frac{ \partial f(\mu)}{\partial x}(x-\mu)$$

对于多维系统,仍旧采用泰勒一阶展开即可得到:

$$T(x) \approx f(a) + (x-a)^T Df(a)$$

其中,$Df(a)$是Jacobian矩阵。

多传感器融合

lidar与radar

本文将以汽车跟踪为例,目标是知道汽车时刻的状态$x = (p_x, p_y, v_x, v_y)$。已知的传感器有lidar、radar。

  • lidar:笛卡尔坐标系。可检测到位置,没有速度信息。其测量值$z=(p_x, p_y)$。
  • radar:极坐标系。可检测到距离,角度,速度信息,但是精度较低。其测量值$z=(\rho, \phi, \dot{\rho})$,图示如下。

这里写图片描述

传感器融合步骤

这里写图片描述

步骤图如上所示,包括:

  1. 收到第一个测量值,对状态$x$进行初始化。
  2. 预测未来
  3. 修正当下

初始化

初始化,指在收到第一个测量值后,对状态$x$进行初始化。初始化如下,同时加上对时间的更新。

对于radar来说,

$$\left[
\begin{matrix}
p_x \\
p_y \\
v_x \\
v_y
\end{matrix}
\right]
= \left[
\begin{matrix}
1 & 0 \\
0 & 1 \\
0 & 0 \\
0 & 0
\end{matrix}
\right] \left[
\begin{matrix}
p_x \\
p_y
\end{matrix}
\right] $$

对于radar来说,

$$\left[
\begin{matrix}
p_x \\
p_y \\
v_x \\
v_y
\end{matrix}
\right]
= \left[
\begin{matrix}
\rho \cos{\phi} \\
\rho \sin{\phi} \\
\dot{\rho} \cos{\phi}\\
\dot{\rho} \sin{\phi}
\end{matrix}
\right] $$

预测未来

预测主要涉及的公式是:

$$\begin{split}
x’ &= Fx \\
P’ &= FPF^T + Q
\end{split}$$

需要求解的有三个变量:$F、P、Q$。


$F$表明了系统的状态如何改变,这里仅考虑线性系统,F易得:

$$Fx = \left[
\begin{matrix}
1 & 0 & dt & 0 \\
0 & 1 & 0& dt \\
0 & 0 & 1& 0 \\
0 & 0 & 0& 1
\end{matrix}
\right]
\left[
\begin{matrix}
p_x \\
p_y \\
v_x \\
v_y
\end{matrix}
\right]$$


$P$表明了系统状态的不确定性程度,用$x$的协方差表示,这里自己指定为:

$$P = \left[
\begin{matrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0& 0 \\
0 & 0 & 1000& 0 \\
0 & 0 & 0& 1000
\end{matrix}
\right]$$


$Q$表明了$x’ = Fx$未能刻画的其他外界干扰。本例子使用线性模型,因此加速度变成了干扰项。$x’ = Fx$中未衡量的额外项目$v$为:

$$v = \left[
\begin{matrix}
\frac{a_x dt^2}{2} \\
\frac{a_y dt^2}{2} \\
a_x dt \\
a_y dt
\end{matrix}
\right] =
\left[
\begin{matrix}
\frac{dt^2}{2} & 0 \\
0 & \frac{dt^2}{2} \\
dt & 0 \\
0 & dt
\end{matrix}
\right]
\left[
\begin{matrix}
a_x\\
a_y
\end{matrix}
\right] = Ga$$

$v$服从高斯分布$N(0, Q)$。

$$\begin{split}
Q &= E[v v^T]= E[Gaa^TG^T] = GE[aa^T]G^T \\
&= G\left[
\begin{matrix}
\sigma_{ax}^2 & 0\\
0 & \sigma_{ay}^2
\end{matrix}
\right] G^T \\
&= \left[
\begin{matrix}
\frac{ {dt}^4}{4} \sigma_{ax}^2 & 0 & \frac{ {dt}^3}{2} \sigma_{ax}^2 & 0 \\
0 & \frac{ {dt}^4}{4} \sigma_{ay}^2 & 0 & \frac{ {dt}^3}{2} \sigma_{ay}^2 \\
\frac{ {dt}^3}{2} \sigma_{ax}^2 & 0 & {dt}^2 \sigma_{ax}^2 & 0 \\
0 & \frac{ {dt}^3}{2} \sigma_{ay}^2 & 0& {dt}^2 \sigma_{ay}^2
\end{matrix}
\right]
\end{split}$$

修正当下

lidar

lidar使用了KF。修正当下这里牵涉到的公式主要是:

$$\begin{split}
y &= z - Hx \\
S &= HPH^T+R \\
K &= PH^TS^{-1} \\
x’ &= x+Ky \\
P’ &= (I-KH)P
\end{split}$$

需要求解的有两个变量:$H、R$。


$H$表示了状态空间到测量空间的映射。

$$Hx = \left[
\begin{matrix}
1 & 0 & 0 & 0\\
0 & 1 & 0 & 0
\end{matrix}
\right]
\left[
\begin{matrix}
p_x \\
p_y \\
v_x \\
v_y
\end{matrix}
\right]
$$


$R$表示了测量值的不确定度,一般由传感器的厂家提供,这里lidar参考如下:

$$R_{laser} = \left[
\begin{matrix}
0.0225 & 0 \\
0 & 0.0225
\end{matrix}
\right]$$

radar

radar使用了EKF。修正当下这里牵涉到的公式主要是:

$$\begin{split}
y &= z - f(x) \\
S &= H_jPH_j^T+R \\
K &= PH_j^TS^{-1} \\
x’ &= x+Ky \\
P’ &= (I-KH_j)P
\end{split}$$

区别与上面lidar的主要有:

  1. 状态空间到测量空间的非线性映射$f(x)$
  2. 非线性映射线性化后的Jacob矩阵
  3. radar的$R_{radar}$

状态空间到测量空间的非线性映射$f(x)$如下

$$f(x) = \left[
\begin{matrix}
\rho \\
\phi \\
\dot{\rho}
\end{matrix}
\right] =
\left[
\begin{matrix}
\sqrt{p_x^2+p_y^2} \\
\arctan{\frac{p_y}{p_x}} \\
\frac{p_x v_x + p_y v_y}{\sqrt{p_x^2+p_y^2} }
\end{matrix}\right]$$


非线性映射线性化后的Jacob矩阵$H_j$

$$H_j = \frac{\partial f(x)}{\partial x} =
\left[
\begin{matrix}
\frac{\partial \rho}{\partial p_x} & \frac{\partial \rho}{\partial p_y} & \frac{\partial \rho}{\partial v_x} & \frac{\partial \rho}{\partial v_y} \\
\frac{\partial \phi}{\partial p_x} & \frac{\partial \phi}{\partial p_y} & \frac{\partial \phi}{\partial v_x} & \frac{\partial \phi}{\partial v_y} \\
\frac{\partial \dot{\rho}}{\partial p_x} & \frac{\partial \dot{\rho}}{\partial p_y} & \frac{\partial \dot{\rho}}{\partial v_x} & \frac{\partial \dot{\rho}}{\partial v_y}
\end{matrix}
\right]$$


$R$表示了测量值的不确定度,一般由传感器的厂家提供,这里radar参考如下:

$$R_{laser} = \left[
\begin{matrix}
0.09 & 0 & 0\\
0 & 0.0009 & 0 \\
0 & 0 & 0.09
\end{matrix}
\right]$$

传感器融合实例

多传感器融合的示例如下,需要注意的有:

  1. lidar和radar的预测部分是完全相同的
  2. lidar和radar的参数更新部分是不同的,不同的原因是不同传感器收到的测量值是不同的
  3. 当收到lidar或radar的测量值,依次执行预测、更新步骤
  4. 当同时收到lidar和radar的测量值,依次执行预测、更新1、更新2步骤

这里写图片描述

多传感器融合的效果如下图所示,红点和蓝点分别表示radar和lidar的测量位置,绿点代表了EKF经过多传感器融合后获取到的测量位置,取得了较低的RMSE。

这里写图片描述