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

这里写图片描述

KF与EKF

本文假定读者已熟悉KF，若不熟悉请参考

卡尔曼滤波简介

。

KF与EKF的区别如下：

预测未来：

$x'=Fx+u$
用

$x'=f(x,u)$
代替；其余

$F$
用

$F_j$
代替。
修正当下：将状态映射到测量的

$Hx'$
用

$h(x')$
代替；其余

$H$
用

$H_j$
代替。

其中，非线性函数

$f(x,u)，h(x')$
用非线性得到了更精准的状态预测值、映射后的测量值；线性变换

$F_j，H_j$
通过线性变换使得变换后的

$x，z$
仍满足高斯分布的假设。

$F_j，H_j$
计算方式如下：

F j b = \partial f ( x , u ) \partial x = \partial h ( x ' ) \partial x

$\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 (μ) + \partial f ( μ ) \partial x (x - μ)

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

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

T (x) \approx f (a) + (x - a) T D f (a)

$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})$
，图示如下。

这里写图片描述

传感器融合步骤

这里写图片描述

步骤图如上所示，包括：

收到第一个测量值，对状态

$x$
进行初始化。
预测未来
修正当下

初始化

初始化，指在收到第一个测量值后，对状态

$x$
进行初始化。初始化如下，同时加上对时间的更新。

对于radar来说，

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ p x p y v x v y ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ 10000100 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ [p x p y]

$\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来说，

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ p x p y v x v y ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ρ cos ϕ ρ sin ϕ ρ ˙ cos ϕ ρ ˙ sin ϕ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥

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

预测未来

预测主要涉及的公式是：

x' P' = F x = F P F T + Q

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

需要求解的有三个变量：

$F、P、Q$
。

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

F x = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ 10000100 d t 010 0 d t 01 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ p x p y v x v y ⎤ ⎦ ⎥ ⎥ ⎥ ⎥

$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 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ 1000010000100000001000 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥

$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 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ a x d t 2 2 a y d t 2 2 a x d t a y d t ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ d t 2 2 0 d t 0 0 d t 2 2 0 d t ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ [a x a y] = G a

$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)$
。

Q = E [v v T] = E [G a a T G T] = G E [a a T] G T = G [σ 2 a x 0 0 σ 2 a y] G T = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ d t 4 4 σ 2 a x 0 d t 3 2 σ 2 a x 0 0 d t 4 4 σ 2 a y 0 d t 3 2 σ 2 a y d t 3 2 σ 2 a x 0 d t 2 σ 2 a x 0 0 d t 3 2 σ 2 a y 0 d t 2 σ 2 a y ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$\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。修正当下这里牵涉到的公式主要是：

y S K x' P' = z - H x = H P H T + R = P H T S - 1 = x + K y = (I - K H) P

$\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$
表示了状态空间到测量空间的映射。

H x = [10010000] ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ p x p y v x v y ⎤ ⎦ ⎥ ⎥ ⎥ ⎥

$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 l a s e r = [0.0225 0 0 0.0225]

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

radar

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

y S K x' P' = z - f (x) = H j P H T j + R = P H T j S - 1 = x + K y = (I - K H j) P

$\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的主要有：

状态空间到测量空间的非线性映射

$f(x)$
非线性映射线性化后的Jacob矩阵
radar的

$R_{radar}$

状态空间到测量空间的非线性映射

$f(x)$
如下

f (x) = ⎡ ⎣ ⎢ ⎢ ρ ϕ ρ ˙ ⎤ ⎦ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ p 2 x + p 2 y ‾ ‾ ‾ ‾ ‾ ‾ ‾ \sqrt arctan p y p x p x v x + p y v y p 2 x + p 2 y ‾ ‾ ‾ ‾ ‾ ‾ ‾ \sqrt ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$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 = \partial f ( x ) \partial x = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ \partial ρ \partial p x \partial ϕ \partial p x \partial ρ ˙ \partial p x \partial ρ \partial p y \partial ϕ \partial p y \partial ρ ˙ \partial p y \partial ρ \partial v x \partial ϕ \partial v x \partial ρ ˙ \partial v x \partial ρ \partial v y \partial ϕ \partial v y \partial ρ ˙ \partial v y ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$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 l a s e r = ⎡ ⎣ ⎢ ⎢ 0.09 00 0 0.0009 0 00 0.09 ⎤ ⎦ ⎥ ⎥

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

传感器融合实例

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

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

这里写图片描述

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

这里写图片描述

原文链接：https://blog.csdn.net/young_gy/article/details/78468153

KF与EKF

为什么要用EKF

将非线性系统线性化

多传感器融合

lidar与radar

传感器融合步骤

初始化

预测未来

修正当下

lidar

radar

传感器融合实例

你可能也喜欢