扩展卡尔曼滤波器是卡尔曼滤波器的非线性版本。 在确定状态转移方程的情况下，EKF已经成为非线性系统状态估计的默认标准。 本文简要介绍EKF，介绍无人驾驶在多传感器融合中的应用。

KF和EKF本文假设读者熟悉KF。 如果不熟悉，请参阅卡尔曼滤波器概述。

KF和EKF的区别如下。

被预测未来的(x ) x’=Fx u被x )=f ) x，u )代替； 剩下的f用Fj代替。 修正当前：将状态映射到测量的hx ‘，替换为h(x )； 剩下的h用Hj代替。 在此，非线性函数f(x，u )、h ) x使用非线性得到更准确的状态预测值、映射后的测量值； 线性变换Fj、Hj满足通过线性变换变换后的x、z满足rydlc分布的假设。

Fj，Fj计算如下。

FJB=f(x，u ) x=h (x ) ) x

为什么要使用EKF KF的一个假设是rydlc分布在x预测后仍然遵循rydlc分布，而rydlc分布在x转换到测量空间后仍然遵循rydlc分布。 但是，如果f、h为非线性变换，则上述条件不成立。

既然非线性系统不能是非线性系统，自然的解决方法是非线性系统的线性化。

一维系统的情况下，使用结实的戒指一个阶段展开就可以得到：

f(x ) f() f() f() ) )

对于多维系统，还是采用结实的戒指展开一层就能得到。

将t(x ) f(a ) xa ) TDF (a )

其中df(a )是Jacobian矩阵。

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

lidar :笛卡尔坐标系。 无法检测位置，没有速度信息。 其测定值z=(px，py )。 radar :极坐标系。 可以检测距离、角度、速度的信息，但精度低。 其测定值z=(，) )如下图示。

传感器融合步骤

步骤图如上所示。

接收第一个测量值，初始化状态x。 未来预测修正的当前初始化是指在收到最初的测量值后，初始化状态x。 如下初始化，同时添加时间更新。

对radar来说，

pxpyvxvy=10000100[pxpy]

对radar来说，

pxpyvxvy=cossin； cos； sin预测未来

与主要预测相关的公式如下

x’p’=FX=FPFTq

需要求解的变量有三个。 是f、p、q。

f显示了系统的状态如何变化。 这里只考虑线性系统。 f很容易得到。

FX=10000100dt0100dt01pxpyvxvy

p表示系统状态的不确定性程度，用x的协方差表示，这里自己指定如下

p=100001000010000000000001000

q表示x’=Fx未能描绘完的其他干扰。 在本例中，由于使用了线性模型，因此加速度为噪声。 未通过x’=Fx测量的附加项目v如下。

v=axdt 22 aydt 22 axdtaydt=dt 220 dt00 dt 220 dt [ axay ]=ga

v遵循rydlc分布n(0，q )。

q=e [ vvt ]=e [ gaat gt ]=ge [ aat ] gt=g [2ax 002ax 0dt 322ax 00 dt 442a y0dt 322a x0dt 32 ax 00 dt 33

lidar使用了KF。 修改目前这里涉及的公式主要有：

ykx ‘ p ‘=zhx=hph tr=ph ts1=xky=(ikh ) p

/p>

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

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

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

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

Rlaser=[0.0225000.0225]

radar

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

ySKx′P′=z−f(x)=HjPHTj+R=PHTjS−1=x+Ky=(I−KHj)P

区别与上面lidar的主要有：

状态空间到测量空间的非线性映射 f(x) 非线性映射线性化后的Jacob矩阵radar的 Rradar

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

f(x)=⎡⎣⎢⎢ρϕρ˙⎤⎦⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢p2x+p2y‾‾‾‾‾‾‾√arctanpypxpxvx+pyvyp2x+p2y‾‾‾‾‾‾‾√⎤⎦⎥⎥⎥⎥⎥⎥⎥

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

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

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

Rlaser=⎡⎣⎢⎢0.090000.00090000.09⎤⎦⎥⎥

传感器融合实例

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

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

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