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

https://blog.csdn.net/Young_Gy/article/details/78468153

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

KF与EKF

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

KF与EKF的区别如下：

1. 预测未来：x′=Fx+ux′=Fx+u用x′=f(x,u)x′=f(x,u)代替；其余FF用FjFj代替。
2. 修正当下：将状态映射到测量的Hx′Hx′用h(x′)h(x′)代替；其余HH用HjHj代替。

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

Fj，HjFj，Hj计算方式如下：

Fjb=∂f(x,u)∂x=∂h(x′)∂xFj=∂f(x,u)∂xb=∂h(x′)∂x

为什么要用EKF

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

将非线性系统线性化

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

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

f(x)≈f(μ)+∂f(μ)∂x(x−μ)f(x)≈f(μ)+∂f(μ)∂x(x−μ)

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

T(x)≈f(a)+(x−a)TDf(a)T(x)≈f(a)+(x−a)TDf(a)

其中，Df(a)Df(a)是Jacobian矩阵。

多传感器融合

lidar与radar

本文将以汽车跟踪为例，目标是知道汽车时刻的状态x=(px,py,vx,vy)x=(px,py,vx,vy)。已知的传感器有lidar、radar。

• lidar：笛卡尔坐标系。可检测到位置，没有速度信息。其测量值z=(px,py)z=(px,py)。
• radar：极坐标系。可检测到距离，角度，速度信息，但是精度较低。其测量值z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙)，图示如下。

传感器融合步骤

步骤图如上所示，包括：

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

初始化

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

对于radar来说，

⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢10000100⎤⎦⎥⎥⎥[pxpy][pxpyvxvy]=[10010000][pxpy]

对于radar来说，

⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢ρcosϕρsinϕρ˙cosϕρ˙sinϕ⎤⎦⎥⎥⎥⎥[pxpyvxvy]=[ρcos⁡ϕρsin⁡ϕρ˙cos⁡ϕρ˙sin⁡ϕ]

预测未来

预测主要涉及的公式是：

x′P′=Fx=FPFT+Qx′=FxP′=FPFT+Q

需要求解的有三个变量：F、P、QF、P、Q。

---

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

Fx=⎡⎣⎢⎢⎢10000100dt0100dt01⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥Fx=[10dt0010dt00100001][pxpyvxvy]

---

PP表明了系统状态的不确定性程度，用xx的协方差表示，这里自己指定为：

P=⎡⎣⎢⎢⎢1000010000100000001000⎤⎦⎥⎥⎥P=[1000010000100000001000]

---

QQ表明了x′=Fxx′=Fx未能刻画的其他外界干扰。本例子使用线性模型，因此加速度变成了干扰项。x′=Fxx′=Fx中未衡量的额外项目vv为：

v=⎡⎣⎢⎢⎢⎢⎢axdt22aydt22axdtaydt⎤⎦⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢dt220dt00dt220dt⎤⎦⎥⎥⎥⎥⎥[axay]=Gav=[axdt22aydt22axdtaydt]=[dt2200dt22dt00dt][axay]=Ga

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

Q=E[vvT]=E[GaaTGT]=GE[aaT]GT=G[σ2ax00σ2ay]GT=⎡⎣⎢⎢⎢⎢⎢⎢dt44σ2ax0dt32σ2ax00dt44σ2ay0dt32σ2aydt32σ2ax0dt2σ2ax00dt32σ2ay0dt2σ2ay⎤⎦⎥⎥⎥⎥⎥⎥Q=E[vvT]=E[GaaTGT]=GE[aaT]GT=G[σax200σay2]GT=[dt44σax20dt32σax200dt44σay20dt32σay2dt32σax20dt2σax200dt32σay20dt2σay2]

修正当下

lidar

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

ySKx′P′=z−Hx=HPHT+R=PHTS−1=x+Ky=(I−KH)Py=z−HxS=HPHT+RK=PHTS−1x′=x+KyP′=(I−KH)P

需要求解的有两个变量：H、RH、R。

---

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

Hx=[10010000]⎡⎣⎢⎢⎢⎢pxpyvxvy⎤⎦⎥⎥⎥⎥Hx=[10000100][pxpyvxvy]

---

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

Rlaser=[0.0225000.0225]Rlaser=[0.0225000.0225]

radar

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

ySKx′P′=z−f(x)=HjPHTj+R=PHTjS−1=x+Ky=(I−KHj)Py=z−f(x)S=HjPHjT+RK=PHjTS−1x′=x+KyP′=(I−KHj)P

区别与上面lidar的主要有：

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

---

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

f(x)=⎡⎣⎢ρϕρ˙⎤⎦⎥=⎡⎣⎢⎢⎢⎢⎢⎢p2x+p2y−−−−−−√arctanpypxpxvx+pyvyp2x+p2y√⎤⎦⎥⎥⎥⎥⎥⎥f(x)=[ρϕρ˙]=[px2+py2arctan⁡pypxpxvx+pyvypx2+py2]

---

非线性映射线性化后的Jacob矩阵HjHj

Hj=∂f(x)∂x=⎡⎣⎢⎢⎢⎢⎢∂ρ∂px∂ϕ∂px∂ρ˙∂px∂ρ∂py∂ϕ∂py∂ρ˙∂py∂ρ∂vx∂ϕ∂vx∂ρ˙∂vx∂ρ∂vy∂ϕ∂vy∂ρ˙∂vy⎤⎦⎥⎥⎥⎥⎥Hj=∂f(x)∂x=[∂ρ∂px∂ρ∂py∂ρ∂vx∂ρ∂vy∂ϕ∂px∂ϕ∂py∂ϕ∂vx∂ϕ∂vy∂ρ˙∂px∂ρ˙∂py∂ρ˙∂vx∂ρ˙∂vy]

---

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

Rlaser=⎡⎣⎢0.090000.00090000.09⎤⎦⎥Rlaser=[0.090000.00090000.09]

传感器融合实例

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

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

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