卡尔曼滤波在自动驾驶传感器融合中的应用

（一）卡尔曼滤波的原理

关于卡尔曼滤波的原理，可以参考Bzarg所写的《How a Kalman filter works, in pictures》，卡尔曼滤波总体框图如下所示。

 

我自己总结卡尔曼滤波主要有以下两个步骤：

1）根据估算值得到预测值；

2）根据测量值对预测值进行更新，得到新的估算值。

（二）GPS和惯性传感器IMU的融合

GPS的缺点是更新频率低，无法满足实时性计算的要求。惯性传感器IMU是高频传感器，在短时间内可以提供稳定的实时位置更新，但是其定位误差随着运行时间增加。所以需要融合这两种传感器的优点，才能得到实时和精准的定位。

如下图所示，使用卡尔曼滤波器对惯性传感器IMU与GPS数据进行融合。首先，我们在上一次位置估算的基础上使用惯性传感器IMU对当前的位置进行实时预测。在得到新的GPS数据之前，我们只能通过积分惯性传感器IMU的数据预测当前位置。惯性传感器IMU的定位误差会随着运行时间增长，所以当接收到新的GPS数据时，由于GPS的数据比较精准，我们可以使用GPS数据对当前的位置预测进行更新。通过不断地执行这两个步骤，可以取两者所长，对无人车进行准确的实时定位。假设惯性传感器的频率是1kHz，而GPS的频率是10Hz，那么每两次GPS更新之间会使用100个惯性传感器数据点进行位置预测。

参考：

1）How a Kalman filter works, in pictures，Bzarg

2）第一本无人驾驶技术书，刘少山等著，北京：电子工业出版社，2017.6