自动驾驶-使用卡尔曼滤波算法定位和跟踪

参加过科一的人都知道，学车的第一步不是操控车辆而是遵守交规，行车礼让，确保安全。可见安全驾驶才是行车的第一原则。为了确保安全，司机应该观察周围车辆和行人的位置，保持安全距离。自动驾驶的车辆用来感知周围的设备是雷达和激光传感器。本章介绍使用c++实现卡尔曼滤波算法融合雷达和激光传感器完成对物体的感知和追踪，是对udacity自动驾驶课程该项目的学习总结。

以感知行人为例，卡尔曼滤波算法和传感器感知物体的一般流程为：

1. 传感器第一次发现行人，用测量数据对一组描述行人的特征向量初始化得到$x_1$；
2. 进入卡尔曼滤波算法的预测阶段，计算$\Delta t$后的特征向量的预测值$x_t^{\prime }$，方差矩阵$P_t^{\prime }$；预测值的不确定性增加，即$P_t^{\prime }$增加。同时传感器继续测量数据；
3. 进入卡尔曼滤波测量更新阶段， 这时卡尔曼滤波算法得到$\Delta t$时间后的测量值，