动手学无人驾驶（5）：多传感器数据融合

本系列的前4篇文章主要介绍了深度学习技术在无人驾驶环境感知中的应用，包括交通标志识别，图像与点云3D目标检测。关于环境感知部分的介绍本系列暂且告一段落，后续如有需要再进行补充。
现在我们开启新的篇章，在本文中将会介绍无人驾驶的定位部分，我们将使用仿真的Radar和LiDAR数据对自行车进行追踪。这里使用到了两种传感器数据，因此我们需要进行数据融合，同时由于两种传感器工作原理不同，我们需要分别应用卡尔曼滤波与扩展卡尔曼滤波技术。
本文主要参考了Udacity《无人驾驶工程师》课程相关项目，同时也参考了知乎作者陈光的分享，在此一同表示感谢。

1. 毫米波雷达简介

毫米波雷达是自动驾驶汽车常用的传感器之一，目前市场上常用的毫米波雷达有大陆ARS408雷达，测距范围最远能达到250米。

毫米波雷达的工作原理是多普勒效应，输出值是极坐标下的测量结果。

如下图所示，毫米波雷达能够测量障碍物在极坐标下的距离$\rho$，方向夹角 $\phi$，以及径向速度$\dot{\rho }$。

---

2. 激光雷达简介

激光雷达与毫米波雷达不同，它的测量原理是光沿直线传播。

能直接获得障碍物在笛卡尔坐标系下$x$方向、$y$方向和$z$方向上的距离。目前市场上常用的激光雷达有Velodyne激光雷达，国内有Robosense激光雷达以及大疆公司旗下的激光雷达。激光雷达根据线束的多少，分为16线，32线，64线，以及128线激光雷达。

---

3. 卡尔曼滤波直观理解

网上介绍卡尔曼滤波原理的书和资料有很多，这里从直观上来对其进行介绍，帮助大家理解，具体数学公式推导可查阅相关论文。

先一句话概括卡尔曼滤波的思想：根据上一时刻的状态$x_{t-1}$，预测当前时刻的状态$x_{pre}$，将预测的状态$x_{pre}$与当前时刻的测量值$z_t$进行加权更新，更新后的结果为最终的追踪结果$x_t$。

以一个常见的小车运动为例来介绍。如下图所示：有辆小车在道路上水平向右侧匀速运动，在左侧$o$点安装了传感器，传感器每隔1秒测量一次小车的位置$s$和运动速度$v$。

这里用向量$x_t$来表示小车在$t$时刻的运动状态，该向量$x_t$也是最终的输出结果，被称作状态向量（state vector），则状态向量为：
$$x_t=\left[\begin{array}{c}{s}_t\\ v_t\end{array}\right]$$

由于传感器自身测量误差的存在，无法直接获取小车的真实位置，只能获取在真值附近的一个近似值，可以假设测量值在真值附近服从高斯分布，如下图所示，橙色部分为测量值。

在初始的时候，由于不存在上一时刻的状态，我们设初始的状态向量为测量值，因此有：

$$x_1=z_1=\left[\begin{array}{c}{s}_1\\ v_1\end{array}\right]$$

---

3.1 预测

卡尔曼滤波大致分为两步，第一步为状态预测（prediction）。现在我们已经有了小车第1秒的状态$x_1$，可以预测小车在第2秒的状态，小车所处位置假设如下图所示。

由于小车是做匀速运动，因此小车在第2秒时的预测状态为：
$$x_{pre}=\left[\begin{array}{c}{s}_1+v_1\\ v_1\end{array}\right]$$

---

3.2 更新

现在我们已经预测了小车在第2秒的状态，同时传感器也测量出小车在第2秒时的位置，测量结果用$z_2$表示，则：
$$z_2=\left[\begin{array}{c}{s}_2\\ v_2\end{array}\right]$$
由于传感器本身存在着测量噪声，测量结果存在很大不确定性，将小车预测位置与测量值进行比较，如下图所示。

不难发现，小车的真实位置应该处于测量值与预测值之间。

对测量值与预测值进行加权，加权后的结果如下图所示，绿色部分即为小车真值分布区域。

这样，根据第2秒的预测值和测量值，我们就能得到第2秒的状态向量$x_2$。同理，按照上述预测、更新的过程，我们就能得到第3秒，第4秒…第n秒的状态向量$x_n$。

上面是卡尔曼滤波的直观理解，下面给出其数学公式。这里留个伏笔，在下一节部分=结合代码再介绍其中每个字母的含义。

---

4. 传感器数据融合

不知道大家有没有想过这样一个问题？既然毫米波雷达与激光雷达都能测量障碍物的位置，为什么还需要对传感器数据进行融合操作呢。

这是因为在自动驾驶中，使用单一传感器进行障碍物跟踪，往往都有着很大的局限性。激光雷达测量更精准，但是其无法得到速度信息；毫米波雷达能够得到障碍物速度信息，但是其位置测量精度不如激光雷达。如果能将二者有效结合，就能精准得到障碍物位置和速度信息，因此数据融合技术孕育而生。

在毫米波雷达与激光雷达进行数据融合时，因为两个传感器工作原理的不同，其相应的技术处理细节也不同。激光雷达可直接使用线性卡尔曼来进行障碍物跟踪；而毫米波雷达则需要使用非线性卡尔曼来进行物体跟踪。

这里分为三个小节来实现，一是线性卡尔曼滤波的实现；二是非线性卡尔曼滤波的实现；三是对二者进行数据融合。

---

4.1 线性卡尔曼滤波实现

在正式写代码处理问题时，我们先熟悉下要处理的传感器数据，这里要处理的是激光雷达与毫米波雷达数据。

下面是激光雷达与毫米波雷达交替发出的前20行数据，其中L代表激光雷达，R代表毫米波雷达。

L	0	0	1477010443349642	0	0	0	0
R	0	0	0	1477010443349642	0	0	0	0
L	1.559445e+00	-1.385015e-01	1477010444349642	2.098967e+00	5.222280e-02	2.195949e+00	1.093391e-01
R	1.812711e+00	2.619727e-02	2.305732e+00	1477010444349642	2.098967e+00	5.222280e-02	2.195949e+00	1.093391e-01
L	3.890927e+00	-1.341657e-01	1477010445349642	4.291359e+00	2.153118e-01	2.284336e+00	2.263225e-01
R	4.200967e+00	5.202598e-02	2.418127e+00	1477010445349642	4.291359e+00	2.153118e-01	2.284336e+00	2.263225e-01
L	6.863517e+00	4.168175e-01	1477010446349642	6.569422e+00	4.960956e-01	2.363816e+00	3.488471e-01
R	6.456604e+00	7.330529e-02	2.438466e+00	1477010446349642	6.569422e+00	4.960956e-01	2.363816e+00	3.488471e-01
L	9.077331e+00	5.932112e-01	1477010447349642	8.924371e+00	8.992403e-01	2.433593e+00	4.745258e-01
R	8.941596e+00	9.936074e-02	2.529122e+00	1477010447349642	8.924371e+00	8.992403e-01	2.433593e+00	4.745258e-01
L	1.157555e+01	1.666900e+00	1477010448349642	1.134677e+01	1.426997e+00	2.493282e+00	6.007812e-01
R	1.153365e+01	1.242681e-01	2.500995e+00	1477010448349642	1.134677e+01	1.426997e+00	2.493282e+00	6.007812e-01
L	1.359209e+01	2.311915e+00	1477010449349642	1.382695e+01	2.079045e+00	2.542900e+00	7.249468e-01
R	1.380271e+01	1.453491e-01	2.539724e+00	1477010449349642	1.382695e+01	2.079045e+00	2.542900e+00	7.249468e-01
L	1.664559e+01	2.902999e+00	1477010450349642	1.635537e+01	2.852433e+00	2.582842e+00	8.443653e-01
R	1.652890e+01	1.730753e-01	2.728349e+00	1477010450349642	1.635537e+01	2.852433e+00	2.582842e+00	8.443653e-01
L	1.901058e+01	3.705553e+00	1477010451349642	1.892299e+01	3.741610e+00	2.613820e+00	9.564796e-01
R	1.974624e+01	1.973267e-01	2.502012e+00	1477010451349642	1.892299e+01	3.741610e+00	2.613820e+00	9.564796e-01
L	2.133124e+01	4.732053e+00	1477010452349642	2.152152e+01	4.738551e+00	2.636790e+00	1.058912e+00
R	2.213645e+01	2.161499e-01	2.798219e+00	1477010452349642	2.152152e+01	4.738551e+00	2.636790e+00	1.058912e+00

激光雷达每一帧有7个数据，依次是：

• 障碍物在X方向上的测量值（单位：米）；
• Y方向上的测量值（单位：米）；
• 测量时刻的时间戳（单位：微秒）；
• 障碍物位置在X方向上的真值（单位：米）；
• 障碍物位置在Y方向上的真值（单位：米）；
• 障碍物速度在X方向上的真值（单位：米/秒）；
• 障碍物速度在Y方向上的真值（单位：米/秒）。

毫米波雷达每一帧有8个数据，依次是：

• 障碍物在极坐标系下的距离（单位：米）；
• 角度（单位：弧度） ；
• 径向速度（单位：米/秒）；
• 测量时刻的时间戳（单位：微秒）；
• 障碍物位置在X方向上的真值（单位：米）；
• 障碍物位置在Y方向上的真值（单位：米）；
• 障碍物速度在X方向上的真值（单位：米/秒）；
• 障碍物速度在Y方向上的真值（单位：米/秒）。

---

对数据有了一定了解后，现在我们开始实现线性卡尔曼滤波。

（1）初始化

首先是初始化部分，在这里要初始化卡尔曼滤波的各个变量。

这里使用Eigen库进行初始化，这里我们定义了一个KalmanFilter类，表示为卡尔曼滤波追踪器，其成员函数包括初始化，预测，线性卡尔曼更新，扩展卡尔曼更新等，这在面向对象编程中是常使用的一种方法。

#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_
#include "Eigen/Dense"

class KalmanFilter {
public:

	// state vector
	 Eigen::VectorXd x_;
	
	 // state covariance matrix
	 Eigen::MatrixXd P_;
	
	 // state transistion matrix
	 Eigen::MatrixXd F_;
	
	 // process covariance matrix
	 Eigen::MatrixXd Q_;
	
	 // measurement matrix
	 Eigen::MatrixXd H_;
	
	 // measurement covariance matrix
	 Eigen::MatrixXd R_;
	
	 /**
	  * Constructor
	  */
	 KalmanFilter();
	
	 /**
	  * Destructor
	  */
	 virtual ~KalmanFilter();
	
	 /**
	  * Init Initializes Kalman filter
	  * @param x_in Initial state
	  * @param P_in Initial state covariance
	  * @param F_in Transition matrix
	  * @param H_in Measurement matrix
	  * @param R_in Measurement covariance matrix
	  * @param Q_in Process covariance matrix
	  */
	 void Init(Eigen::VectorXd &x_in, Eigen::MatrixXd &P_in, Eigen::MatrixXd &F_in,
	     Eigen::MatrixXd &H_in, Eigen::MatrixXd &R_in, Eigen::MatrixXd &Q_in);
	
	 /**
	  * Prediction Predicts the state and the state covariance
	  * using the process model
	  * @param delta_T Time between k and k+1 in s
	  */
	 void Predict();
	
	 /**
	  * Updates the state by using standard Kalman Filter equations
	  * @param z The measurement at k+1
	  */
	 void Update(const Eigen::VectorXd &z);
	
	 /**
	  * Updates the state by using Extended Kalman Filter equations
	  * @param z The measurement at k+1
	  */
	 void UpdateEKF(const Eigen::VectorXd &z);
	
	 /**
	 * General kalman filter update operations
	 * @param y the update prediction error
	 */
	 void UpdateRoutine(const Eigen::VectorXd &y);

};

#endif /* KALMAN_FILTER_H_ */

初始化时，需要初始化状态向量$x$，状态转移矩阵$F$，状态协方差矩阵$P$，测量矩阵$H$，过程协方差矩阵$Q$，测量协方差矩阵$R$，代码如下：

void KalmanFilter::Init(VectorXd &x_in, MatrixXd &P_in, MatrixXd &F_in,
						MatrixXd &H_in, MatrixXd &R_in, MatrixXd &Q_in) {
	x_ = x_in;
	P_ = P_in;
	F_ = F_in;
	H_ = H_in;
	R_ = R_in;
	Q_ = Q_in;
}

---

（2）预测

然后是预测部分，根据卡尔曼滤波原理，预测公式为：
$$x^{\prime }=Fx+u$$
这里的$x$为状态向量，其乘以状态转移矩阵$F$，再加上外部影响$u$，即可得到预测状态向量$x^{\prime }$。

对于激光雷达来说，其状态向量$x$为4维向量，$x,y$方向上的位置和速度：
$$x=\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]$$
假设障碍物做匀速运动，则在经过$\delta t$时间后，状态向量为：
$$x^{\prime }=\left[\begin{array}{c}x+v_x\ast \delta t\\ y+v_y\ast \delta t\\ v_x\\ v_y\end{array}\right]$$
如果用矩阵表示的话，则状态转移公式为：
$$\left[\begin{array}{c}x+v_x\ast \delta t\\ y+v_y\ast \delta t\\ v_x\\ v_y\end{array}\right]=\left[\begin{array}{cccc}1& 0& \delta t& 0\\ 0& 1& 0& \delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]$$
因此，状态转移矩阵为$(4,4)$的矩阵，我们可以先将$\delta t$定义为1，计算时再根据实际情况修改：

  // Initial transition matrix F_
  ekf_.F_ = MatrixXd(4, 4);
  ekf_.F_ << 1, 0, 1, 0,
			 0, 1, 0, 1,
			 0, 0, 1, 0,
			 0, 0, 0, 1;

然后是预测部分的第二个公式：
$$P^{\prime }=FP{F}^T+Q$$
在公式中，$P$为状态协方差矩阵，表示系统的不确定性，开始时系统的不确定性会很大，但随着输入的数据越来越多，系统会趋于稳定。这里还用到了过程协方差矩阵$Q$，这表示系统受外部环境影响的情况，如突然遇到爬坡或路面有凹坑的情况。

对于激光雷达来说，无法测量障碍物的速度，因此，测量位置的不确定性要小于速度不确定性。因此这里的$P$可以初始化为：

// Initialize state covariance matrix P
  ekf_.P_ = MatrixXd(4, 4);
  ekf_.P_ << 1,	0,	0,	 0,
			 0,	1,	0,	 0,
			 0,	0, 1000, 0,
			 0,	0, 0,	1000;

$Q$代码如下，这里也是可以调整的。

 // Initialize process noise covariance matrix
  ekf_.Q_ = MatrixXd(4, 4); 
  ekf_.Q_ << 0, 0, 0, 0,
			 0, 0, 0, 0,
			 0, 0, 0, 0,
			 0, 0, 0, 0;

这样我们就完成了预测部分，最终我们写为一个函数Predict()代码如下：

void KalmanFilter::Predict() {

	// Predict the state
	x_ = F_ * x_;
	MatrixXd Ft = F_.transpose();
	P_ = F_ * P_ * Ft + Q_;
}

---

（3）更新

最后是卡尔曼滤波的更新部分，更新部分第一个公式为：
$$y=z-H{x}^{\prime }$$
上式计算了测量值$z$与预测值$x^{\prime }$之间的差值$y$。

由于激光雷达测量值为$(x_m，y_m)$，与状态向量维度不同。在与状态向量进行计算时，需要乘以一个测量矩阵$H$变为同维的状态量，上式用矩阵表示为：
$$\left[\begin{array}{c}\delta x\\ \delta y\end{array}\right]=\left[\begin{array}{c}{x}_m\\ y_m\end{array}\right]-\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\ast \left[\begin{array}{c}x+v_x\ast \delta t\\ y+v_y\ast \delta t\\ v_x\\ v_y\end{array}\right]$$
因此这里的测量矩阵$H$为：

 // Lidar - measurement matrix
  H_laser_ = MatrixXd(2, 4);
  H_laser_ << 1, 0, 0, 0,
			  0, 1, 0, 0;

然后是下面两个公式：
$$\begin{gathered} S=H{P}^{\prime }{H}^T+R \\ K=P^{\prime }{H}^T{S}^{-1} \end{gathered}$$
这两个公式是求卡尔曼增益$K$，实际就是$y$的权重。

最后还有两个公式：
$$\begin{gathered} x=x^{\prime }+Ky \\ P=(I-KH)P^{\prime } \end{gathered}$$
这样就得到了当前帧的状态向量与状态协方差矩阵。然后根据新的状态向量$x$和协方差矩阵$P$可以计算下一帧的状态向量，如此反复。
这里定义测量协方差矩阵$R$为：

 // Initialize measurement covariance matrix - laser
  R_laser_ = MatrixXd(2, 2);
  R_laser_ << 0.0225, 0,
			  0, 0.0225;

至此，卡尔曼滤波中五个重要矩阵$F，P，Q，H，R$就已经定义完了，更新部分代码为：

void KalmanFilter::Update(const VectorXd &z) {

	VectorXd z_pred = H_ * x_;
	VectorXd y = z - z_pred;

	UpdateRoutine(y);
}

void KalmanFilter::UpdateRoutine(const VectorXd &y) {

	MatrixXd Ht = H_.transpose();
	MatrixXd S = H_ * P_ * Ht + R_;
	MatrixXd Si = S.inverse();

	// Compute Kalman gain
	MatrixXd K = P_ * Ht * Si;

	// Update estimate
	x_ = x_ + K * y;
	long x_size = x_.size();
	MatrixXd I = MatrixXd::Identity(x_size, x_size);
	P_ = (I - K * H_) * P_;
}

至此，线性卡尔曼滤波部分就已经完成了，下一节我们实现非线性卡尔曼滤波。

---

4.2 非线性卡尔曼滤波实现

非线性卡尔曼滤波是用于解决非线性问题，与线性卡尔曼滤波相同，非线性卡尔曼滤波也分为初始化、预测、更新三部分。

初始化与线性卡尔曼滤波相似，我们仍然使用KalmanFilter类构造一个追踪器。

（1）预测

这里再回顾下卡尔曼滤波预测中的两个公式：
$$\begin{gathered} x^{\prime }=Fx+u \\ {P}^{\prime }=FP{F}^T+Q \end{gathered}$$
这里仍然使用第一次测量值作为初始状态，状态转移矩阵为$F$，状态协方差矩阵为$P$，过程协方差矩阵为$Q$，与上一节的初始化相同。

---

（2）更新

回顾上一节，更新部分第一个公式为：
$$y=z-H{x}^{\prime }$$
但是这里我们使用的是毫米波雷达数据，测量得到的数据为：距离$\rho$，方向夹角 $\phi$，以及径向速度$\dot{\rho }$。

这里得到的信息为极坐标信息，需要转换为直角坐标距离。我们将上式写成矩阵形式为：
$$y=\left[\begin{array}{c}\rho \\ \\ \phi \\ \\ \dot{\rho }\end{array}\right]-\left[\begin{array}{c}\sqrt{{\rho }_x^2+{\rho }_y^2}\\ \\ arctan(\frac{{\rho }_y}{{\rho }_x})\\ \\ \frac{{\rho }_x{v}_x+{\rho }_y{v}_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}\end{array}\right]$$

其中，${\rho }_x，{\rho }_y$为预测后的位置，$v_x，v_y$为预测后的速度。这里的位置转换，速度转换为非线性转换。

然后是卡尔曼滤波的后面两个公式：
$$\begin{gathered} S=H{P}^{\prime }{H}^T+R \\ K=P^{\prime }{H}^T{S}^{-1} \end{gathered}$$

这里在求卡尔曼增益时，需要知道测量矩阵$H$，因为我们测量数据为$(3,1)$的列向量，而状态向量为$(4,1)$的列向量。因此测量矩阵的维度为$(3,4)$。我们可以写成下面这个形式：
$$H{x}^{\prime }=\left[\begin{array}{c}\sqrt{{\rho }_x^2+{\rho }_y^2}\\ \\ arctan(\frac{{\rho }_y}{{\rho }_x})\\ \\ \frac{{\rho }_x{v}_x{\rho }_y{v}_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}\end{array}\right]=\left[\begin{array}{cccc}{H}_{00}& H_{01}& H_{02}& H_{03}\\ H_{10}& H_{11}& H_{12}& H_{13}\\ H_{20}& H_{21}& H_{22}& H_{23}\end{array}\right]\ast \left[\begin{array}{c}{\rho }_x\\ {\rho }_y\\ v_x\\ v_y\end{array}\right]$$
显然，上式两边转化为非线性转换，那么如何才能将非线性函数用近似线性函数表达呢？

这里我们使用泰勒公式来近似，近似后的泰勒公式为：
$$h(x)\approx h(x_0)+\frac{\partial h(x_0)}{\partial x}(x-x_0)$$
这里忽略了二次以上的高阶项。对$x$求导后的项为雅克比公式。这里，雅克比公式就是我们的测量矩阵$H$。

最终测量矩阵为：
$$H=\left[\begin{array}{cccc}\frac{p_x}{\sqrt{{\rho }_x^2+{\rho }_y^2}}& \frac{p_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}& 0& 0\\ & & & \\ \frac{-p_y}{{\rho }_x^2+{\rho }_y^2}& \frac{-p_x}{{\rho }_x^2+{\rho }_y^2}& 0& 0\\ & & & \\ \frac{p_y(v_x{\rho }_y-v_y{\rho }_x)}{({\rho }_x^2+{\rho }_y^2{)}^{3/2}}& \frac{p_x(v_y{\rho }_x-v_x{\rho }_y)}{({\rho }_x^2+{\rho }_y^2{)}^{3/2}}& \frac{p_x}{\sqrt{{\rho }_x^2+{\rho }_y^2}}& \frac{p_y}{\sqrt{{\rho }_x^2+{\rho }_y^2}}\end{array}\right]$$
这里对测量矩阵进行初始化$H$：

  // Radar - jacobian matrix
  Hj_ = MatrixXd(3, 4);
  Hj_ << 0, 0, 0, 0,
	     0, 0, 0, 0,
		 0, 0, 0, 0;

测量噪声协方差矩阵$R$为：

  //measurement covariance matrix - radar
  R_radar_ = MatrixXd(3, 3);
  R_radar_ << 0.09,	0,		0,
			  0,	0.0009, 0,
			  0,	0,		0.09;

下面是雅克比公式计算函数：

MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) {

	MatrixXd Hj(3, 4);
	
	// Unroll state parameters
	float px = x_state(0);
	float py = x_state(1);
	float vx = x_state(2);
	float vy = x_state(3);
	
	// Pre-compute some term which recur in the Jacobian
	float c1 = px * px + py * py;
	float c2 = sqrt(c1);
	float c3 = c1 * c2;
	
	// Sanity check to avoid division by zero
	if (std::abs(c1) < 0.0001) {
		std::cout << "Error in CalculateJacobian. Division by zero." << std::endl;
		return Hj;
	}
	
	// Actually compute Jacobian matrix
	Hj << (px / c2),				(py / c2),					0,			0,
		-(py / c1),					(px / c1),					0,			0,
		py * (vx*py - vy*px) / c3,	px * (vy*px - vx*py) / c3,	px / c2,	py / c2;
	
	return Hj;

}

最后还有两个公式：
$$\begin{gathered} x=x^{\prime }+Ky \\ P=(I-KH)P^{\prime } \end{gathered}$$
最终得到更新后的状态向量$x$，以及新的状态协方差矩阵$P$。

至此，非线性卡尔曼滤波部分就已经完成了，重点是测量矩阵H的计算。

---

4.3 数据融合

完成了毫米波雷达与激光雷达的预测与更新，现在是对二者进行融合。

下图所示为毫米波雷达与激光雷达数据融合的整体框架。
首先是读入传感器数据，选择第一帧作为初始值。对于之后的帧，进行状态预测，然后根据传感器类型进行更新，最后得到跟踪后的状态向量。

这里定义了一个数据融合类FusionEKF。

class FusionEKF {

public:

	/* Constructor. */
	 FusionEKF();
	
	 /* Destructor. */
	 virtual ~FusionEKF();
	
	 /* Run the whole flow of the Kalman Filter from here. */
	 void ProcessMeasurement(const MeasurementPackage &measurement_pack);
	
	 /* Kalman Filter update and prediction math lives in here. */
	 KalmanFilter ekf_;

private:

	// check whether the tracking toolbox was initialized or not (first measurement)
	 bool is_initialized_;
	
	 // previous timestamp
	 long long previous_timestamp_;
	
	 // tool object used to compute Jacobian and RMSE
	 Tools tools;
	
	 Eigen::MatrixXd R_laser_;
	 Eigen::MatrixXd R_radar_;
	 Eigen::MatrixXd H_laser_;
	 Eigen::MatrixXd Hj_;
	
	 float noise_ax;
	 float noise_ay;
};

上面有一个很重要的处理函数ProcessMeasurement()，是对上图数据融合的代码实现。

void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {

	 /*****************************************************************************
	  *  Initialization
	  ****************************************************************************/
	 if (!is_initialized_){
	 	if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
			// Extract values from measurement
			float rho		= measurement_pack.raw_measurements_(0);
			float phi		= measurement_pack.raw_measurements_(1);
			float rho_dot	= measurement_pack.raw_measurements_(2);
			// Convert from polar to cartesian coordinates
			float px = rho * cos(phi);
			float py = rho * sin(phi);
			float vx = rho_dot * cos(phi);
			float vy = rho_dot * sin(phi);
	
			// Initialize state
			ekf_.x_ << px, py, vx, vy;
	   	}
	   	else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
	
			// Extract values from measurement
			float px = measurement_pack.raw_measurements_(0);
			float py = measurement_pack.raw_measurements_(1);
	
			// Initialize state
			ekf_.x_ << px, py, 0.0, 0.0;
	   	}
	
		// Update last measurement
		previous_timestamp_ = measurement_pack.timestamp_;
	
		// Done initializing, no need to predict or update
	    is_initialized_ = true;
	   return;
	 }
	
	 /*****************************************************************************
	  *  Prediction
	  ****************************************************************************/
	
	 // Compute elapsed time from last measurement
	 float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
	
	 // Update last measurement
	 previous_timestamp_ = measurement_pack.timestamp_;
	
	 // Update state transition matrix F (according to elapsed time dt)
	 ekf_.F_(0, 2) = dt;
	 ekf_.F_(1, 3) = dt;
	
	 // Compute process noise covariance matrix
	 float dt_2 = dt * dt;
	 float dt_3 = dt_2 * dt;
	 float dt_4 = dt_3 * dt;
	
	 ekf_.Q_	<<	dt_4 / 4 * noise_ax,	0,						dt_3 / 2 * noise_ax,	0,
				0,						dt_4 / 4 * noise_ay,	0,						dt_3 / 2 * noise_ay,
				dt_3 / 2 * noise_ax,	0,						dt_2 * noise_ax,		0,
				0,						dt_3 / 2 * noise_ay,	0,						dt_2 * noise_ay;
	
	 ekf_.Predict();
	
	 /*****************************************************************************
	  *  Update
	  ****************************************************************************/
	
	 if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
	 	// Radar updates
	    ekf_.R_ = R_radar_;
	    ekf_.H_ = tools.CalculateJacobian(ekf_.x_);
	    ekf_.UpdateEKF(measurement_pack.raw_measurements_);
	 } else {
	    // Laser updates
	    ekf_.R_ = R_laser_;
	    ekf_.H_ = H_laser_;
	    ekf_.Update(measurement_pack.raw_measurements_);
	 }
	
	 // print the output
	 cout << "x_ = " << ekf_.x_ << endl;
	 cout << "P_ = " << ekf_.P_ << endl;
}

至此，激光雷达与毫米波雷达融合部分就已经完成了。

而在实际开发时，一辆ADAS汽车通常会安装多个毫米波雷达，包括前雷达，后雷达，角雷达；有的还会装有激光雷达；本文没有考虑相机，而相机是ADAS汽车最常使用的传感器。在对这些传感器进行数据融合时还需要考虑时间同步与空间同步问题。

以上只是对汽车外部的障碍物进行定位，而如果汽车想对自身进行定位的话，通常需要安装惯性测量单元IMU和GPS传感器，在下一篇博客中将会进行介绍。