Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下,EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍EKF,并介绍其在无人驾驶多传感器融合上的应用。 KF与EKF本文假定读者已熟悉KF,若不熟悉请参考卡尔曼滤波简介。 KF与EKF的区别如下:
其中,非线性函数f(x,u),h(x′)用非线性得到了更精准的状态预测值、映射后的测量值;线性变换Fj,,Hj通过线性变换使得变换后的x,z仍满足高斯分布的假设。 Fj,Hj计算方式如下: 为什么要用EKFKF的假设之一就是高斯分布的x预测后仍服从高斯分布,高斯分布的x变换到测量空间后仍服从高斯分布。可是,假如F、H是非线性变换,那么上述条件则不成立。 将非线性系统线性化既然非线性系统不行,那么很自然的解决思路就是将非线性系统线性化。 对于一维系统,采用泰勒一阶展开即可得到: 对于多维系统,仍旧采用泰勒一阶展开即可得到: 其中,Df(a)是Jacobian矩阵。 多传感器融合lidar与radar本文将以汽车跟踪为例,目标是知道汽车时刻的状态 。已知的传感器有lidar、radar。
。
,图示如下。 传感器融合步骤步骤图如上所示,包括:
初始化初始化,指在收到第一个测量值后,对状态x进行初始化。初始化如下,同时加上对时间的更新。 对于radar来说, 对于radar来说, 预测未来预测主要涉及的公式是: 需要求解的有三个变量:F、P、Q。 F表明了系统的状态如何改变,这里仅考虑线性系统,F易得: P表明了系统状态的不确定性程度,用x的协方差表示,这里自己指定为: Q表明了x′=Fx未能刻画的其他外界干扰。本例子使用线性模型,因此加速度变成了干扰项。x′=Fx中未衡量的额外项目v为: v服从高斯分布N(0,Q)。 修正当下lidarlidar使用了KF。修正当下这里牵涉到的公式主要是: 需要求解的有两个变量:H、R。 H表示了状态空间到测量空间的映射。 R表示了测量值的不确定度,一般由传感器的厂家提供,这里lidar参考如下: radarradar使用了EKF。修正当下这里牵涉到的公式主要是: 区别与上面lidar的主要有:
状态空间到测量空间的非线性映射f(x)如下 非线性映射线性化后的Jacob矩阵Hj R表示了测量值的不确定度,一般由传感器的厂家提供,这里radar参考如下: 传感器融合实例多传感器融合的示例如下,需要注意的有:
多传感器融合的效果如下图所示,红点和蓝点分别表示radar和lidar的测量位置,绿点代表了EKF经过多传感器融合后获取到的测量位置,取得了较低的RMSE。 |
|