扩展卡尔曼滤波器是卡尔曼滤波器的非线性版本。 在确定状态转移方程的情况下，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。