多传感器融合定位：两小时速览

状态估计(State Estimation)选用传感器需要考虑哪些因素：

1）误差不相关性。也就是说，用于Sensor Fusion的传感器其中单个传感器(Sensor Measurement)测量失败，不会导致其它传感器(Sensor)由于相同的原因而同时失败。

2）传感器的相互补充性。 比如IMU可以填充GPS两次定位间隔期间的定位输出，用于平滑GPS/GNSS的定位结果；GPS为IMU提供初值，消除IMU单独使用出现的偏移(Drift)的问题；Lidar可以弥补定位精度的问题，而GNSS可以为Lidar定位地图匹配提供地图范围数据。

传感器的标定(Sensor Calibration)

如果想要各个传感器能够相互协同，无间配合，传感器的标定是必不可少的。传感器的标定通常分为三种: 内参标定(Intrinsic Calibration)、外参标定(Extrinsic Calibration)和时间校准(Temporal Calibration)。

自动驾驶汽车一般包含多个Camera、3D 激光雷达(Lidar)、惯性测量单元(IMU)、多个Radar、GPS/GNSS Reciver、轮速计(Wheel Odmetry)，这些传感器在运行过程中时刻都在以不同的频率发送不同类型的数据，多传感器融合模块需要将这些信息融合起来，不断更新自动驾驶车辆的状态(Vehicle State)。多传感器融合进行状态估计(State Estimation)的流程如下：

车辆运动模型(Motion Model Input)如下，它的信息一般来自于IMU，包含x、y、z三个方向上的加速度和角速度，是一个6维向量。

EKF的IMU+GNSS+Lidar多传感器融合流程如下:

1）Update State With IMU Inputs   使用IMU的信息 用运动模型来更新状态

2 、 Propagate Uncertainty  不确定性更新

3、当有GNSS或者LIDAR测量结果到达时，进入步骤4），否则进入步骤1）。

4、计算GNSS/Lidar的卡尔曼增益(Kalman Gain)。   使用传感器测量模型H和不确定性P

4、计算Error State。  计算状态误差

5、Correct Predicted State。   状态预测

6、Compute Corrected Covariance。 不确定性预测