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

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

文章目录

1. 毫米波雷达简介
2. 激光雷达简介
3. 卡尔曼滤波直观理解
- 3.1 预测
- 3.2 更新
4. 传感器数据融合
- 4.1 线性卡尔曼滤波实现
- - （1）初始化
  - （2）预测
  - （3）更新
- 4.2 非线性卡尔曼滤波实现
- - （1）预测
  - （2）更新
- 4.3 数据融合

1. 毫米波雷达简介

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

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

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

2. 激光雷达简介

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

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

3. 卡尔曼滤波直观理解

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

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

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

这里用向量xtx_txt来表示小车在ttt时刻的运动状态，该向量xtx_txt也是最终的输出结果，被称作状态向量（state vector），则状态向量为：
xt=[stvt]x_t=\begin{bmatrix}s_t \\ v_t\end{bmatrix}xt=[stvt]

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

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

x1=z1=[s1v1]x_1=z_1=\begin{bmatrix}s_1 \\ v_1\end{bmatrix}x1=z1=[s1v1]

3.1 预测

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

由于小车是做匀速运动，因此小车在第2秒时的预测状态为：
xpre=[s1+v1v1]x_{pre}=\begin{bmatrix}s_1 + v_1\\ v_1\end{bmatrix}xpre=[s1+v1v1]

3.2 更新

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

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

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

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

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

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 vectorEigen::VectorXd x_;// state covariance matrixEigen::MatrixXd P_;// state transistion matrixEigen::MatrixXd F_;// process covariance matrixEigen::MatrixXd Q_;// measurement matrixEigen::MatrixXd H_;// measurement covariance matrixEigen::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_ */

初始化时，需要初始化状态向量xxx，状态转移矩阵FFF，状态协方差矩阵PPP，测量矩阵HHH，过程协方差矩阵QQQ，测量协方差矩阵RRR，代码如下：

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′=Fx+ux'=Fx+ux′=Fx+u
这里的xxx为状态向量，其乘以状态转移矩阵FFF，再加上外部影响uuu，即可得到预测状态向量x′x'x′。

对于激光雷达来说，其状态向量xxx为4维向量，x,yx,yx,y方向上的位置和速度：
x=[xyvxvy]x=\begin{bmatrix}x\\ y\\v_x\\v_y\end{bmatrix}x=⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
假设障碍物做匀速运动，则在经过δt\delta{t}δt时间后，状态向量为：
x′=[x+vx∗δty+vy∗δtvxvy]x'=\begin{bmatrix}{x+v_x*\delta{t}} \\ {y+v_y*\delta{t}}\\v_x\\v_y\end{bmatrix}x′=⎣⎢⎢⎡x+vx∗δty+vy∗δtvxvy⎦⎥⎥⎤
如果用矩阵表示的话，则状态转移公式为：
[x+vx∗δty+vy∗δtvxvy]=[10δt0010δt00100001]∗[xyvxvy]\begin{bmatrix}{x+v_x*\delta{t}} \\ {y+v_y*\delta{t}}\\v_x\\v_y\end{bmatrix}=\begin{bmatrix}1&0&\delta{t}&0\\0&1&0&\delta{t}\\0&0&1&0\\0&0&0&1\end{bmatrix}*\begin{bmatrix}x \\ y \\ v_x \\v_y \end{bmatrix}⎣⎢⎢⎡x+vx∗δty+vy∗δtvxvy⎦⎥⎥⎤=⎣⎢⎢⎡10000100δt0100δt01⎦⎥⎥⎤∗⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
因此，状态转移矩阵为(4,4)(4,4)(4,4)的矩阵，我们可以先将δt\delta{t}δ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′=FPFT+QP'=FPF^T+QP′=FPFT+Q
在公式中，PPP为状态协方差矩阵，表示系统的不确定性，开始时系统的不确定性会很大，但随着输入的数据越来越多，系统会趋于稳定。这里还用到了过程协方差矩阵QQQ，这表示系统受外部环境影响的情况，如突然遇到爬坡或路面有凹坑的情况。

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

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

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

 // Initialize process noise covariance matrixekf_.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 statex_ = F_ * x_;MatrixXd Ft = F_.transpose();P_ = F_ * P_ * Ft + Q_;
}

（3）更新

最后是卡尔曼滤波的更新部分，更新部分第一个公式为：
y=z−Hx′y=z-Hx'y=z−Hx′
上式计算了测量值zzz与预测值x′x'x′之间的差值yyy。

由于激光雷达测量值为(xm，ym)(x_m，y_m)(xm，ym)，与状态向量维度不同。在与状态向量进行计算时，需要乘以一个测量矩阵HHH变为同维的状态量，上式用矩阵表示为：
[δxδy]=[xmym]−[10000100]∗[x+vx∗δty+vy∗δtvxvy]\begin{bmatrix}{\delta{x}} \\ \delta{y}\end{bmatrix}=\begin{bmatrix}x_m\\y_m\end{bmatrix}-\begin{bmatrix}1&0&0&0 \\0&1&0&0 \end{bmatrix}*\begin{bmatrix} {x+v_x*\delta{t}} \\ {y+v_y*\delta{t}}\\v_x\\v_y\end{bmatrix}[δxδy]=[xmym]−[10010000]∗⎣⎢⎢⎡x+vx∗δty+vy∗δtvxvy⎦⎥⎥⎤
因此这里的测量矩阵HHH为：

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

然后是下面两个公式：
S=HP′HT+RK=P′HTS−1S=HP'H^T+R\\K=P'H^TS^{-1}S=HP′HT+RK=P′HTS−1
这两个公式是求卡尔曼增益KKK，实际就是yyy的权重。

最后还有两个公式：
x=x′+KyP=(I−KH)P′x=x'+Ky\\P=(I-KH)P'x=x′+KyP=(I−KH)P′
这样就得到了当前帧的状态向量与状态协方差矩阵。然后根据新的状态向量xxx和协方差矩阵PPP可以计算下一帧的状态向量，如此反复。
这里定义测量协方差矩阵RRR为：

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

至此，卡尔曼滤波中五个重要矩阵F，P，Q，H，RF，P，Q，H，RF，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 gainMatrixXd K = P_ * Ht * Si;// Update estimatex_ = 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）预测

这里再回顾下卡尔曼滤波预测中的两个公式：
x′=Fx+uP′=FPFT+Qx'=Fx+u\\P'=FPF^T+Qx′=Fx+uP′=FPFT+Q
这里仍然使用第一次测量值作为初始状态，状态转移矩阵为FFF，状态协方差矩阵为PPP，过程协方差矩阵为QQQ，与上一节的初始化相同。

（2）更新

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

这里得到的信息为极坐标信息，需要转换为直角坐标距离。我们将上式写成矩阵形式为：
y=[ρφρ˙]−[ρx2+ρy2arctan(ρyρx)ρxvxρyvyρx2+ρy2]y=\begin{bmatrix}\rho\\\\\varphi\\\\\dot{\rho}\end{bmatrix}-\begin{bmatrix}\sqrt{\rho_x^2+\rho_y^2} \\\\arctan(\frac{\rho_y}{\rho_x})\\\\\frac{\rho_xv_x\rho_yv_y}{\sqrt{\rho_x^2+\rho_y^2} } \end{bmatrix}y=⎣⎢⎢⎢⎢⎡ρφρ˙⎦⎥⎥⎥⎥⎤−⎣⎢⎢⎢⎢⎢⎢⎡ρx2+ρy2arctan(ρxρy)ρx2+ρy2ρxvxρyvy⎦⎥⎥⎥⎥⎥⎥⎤

其中，ρx，ρy\rho_x，\rho_yρx，ρy为预测后的位置，vx，vyv_x，v_yvx，vy为预测后的速度。这里的位置转换，速度转换为非线性转换。

然后是卡尔曼滤波的后面两个公式：
S=HP′HT+RK=P′HTS−1S=HP'H^T+R\\K=P'H^TS^{-1}S=HP′HT+RK=P′HTS−1

这里在求卡尔曼增益时，需要知道测量矩阵HHH，因为我们测量数据为(3,1)(3,1)(3,1)的列向量，而状态向量为(4,1)(4,1)(4,1)的列向量。因此测量矩阵的维度为(3,4)(3,4)(3,4)。我们可以写成下面这个形式：
Hx′=[ρx2+ρy2arctan(ρyρx)ρxvxρyvyρx2+ρy2]=[H00H01H02H03H10H11H12H13H20H21H22H23]∗[ρxρyvxvy]Hx'=\begin{bmatrix}\sqrt{\rho_x^2+\rho_y^2} \\\\arctan(\frac{\rho_y}{\rho_x})\\\\\frac{\rho_xv_x\rho_yv_y}{\sqrt{\rho_x^2+\rho_y^2} } \end{bmatrix}=\begin{bmatrix}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{bmatrix}*\begin{bmatrix}\rho_x\\\rho_y\\v_x\\v_y\end{bmatrix}Hx′=⎣⎢⎢⎢⎢⎢⎢⎡ρx2+ρy2arctan(ρxρy)ρx2+ρy2ρxvxρyvy⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎡H00H10H20H01H11H21H02H12H22H03H13H23⎦⎤∗⎣⎢⎢⎡ρxρyvxvy⎦⎥⎥⎤
显然，上式两边转化为非线性转换，那么如何才能将非线性函数用近似线性函数表达呢？

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

最终测量矩阵为：
H=[pxρx2+ρy2pyρx2+ρy200−pyρx2+ρy2−pxρx2+ρy200py(vxρy−vyρx)(ρx2+ρy2)3/2px(vyρx−vxρy)(ρx2+ρy2)3/2pxρx2+ρy2pyρx2+ρy2]H = \begin{bmatrix} \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{bmatrix}H=⎣⎢⎢⎢⎢⎢⎢⎡ρx2+ρy2pxρx2+ρy2−py(ρx2+ρy2)3/2py(vxρy−vyρx)ρx2+ρy2pyρx2+ρy2−px(ρx2+ρy2)3/2px(vyρx−vxρy)00ρx2+ρy2px00ρx2+ρy2py⎦⎥⎥⎥⎥⎥⎥⎤
这里对测量矩阵进行初始化HHH：

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

测量噪声协方差矩阵RRR为：

  //measurement covariance matrix - radarR_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 parametersfloat 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 Jacobianfloat c1 = px * px + py * py;float c2 = sqrt(c1);float c3 = c1 * c2;// Sanity check to avoid division by zeroif (std::abs(c1) < 0.0001) {std::cout << "Error in CalculateJacobian. Division by zero." << std::endl;return Hj;}// Actually compute Jacobian matrixHj << (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;}

最后还有两个公式：
x=x′+KyP=(I−KH)P′x=x'+Ky\\P=(I-KH)P'x=x′+KyP=(I−KH)P′
最终得到更新后的状态向量xxx，以及新的状态协方差矩阵PPP。

至此，非线性卡尔曼滤波部分就已经完成了，重点是测量矩阵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 timestamplong long previous_timestamp_;// tool object used to compute Jacobian and RMSETools 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 measurementfloat 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 coordinatesfloat px = rho * cos(phi);float py = rho * sin(phi);float vx = rho_dot * cos(phi);float vy = rho_dot * sin(phi);// Initialize stateekf_.x_ << px, py, vx, vy;}else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {// Extract values from measurementfloat px = measurement_pack.raw_measurements_(0);float py = measurement_pack.raw_measurements_(1);// Initialize stateekf_.x_ << px, py, 0.0, 0.0;}// Update last measurementprevious_timestamp_ = measurement_pack.timestamp_;// Done initializing, no need to predict or updateis_initialized_ = true;return;}/******************************************************************************  Prediction****************************************************************************/// Compute elapsed time from last measurementfloat dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;// Update last measurementprevious_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 matrixfloat 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 updatesekf_.R_ = R_radar_;ekf_.H_ = tools.CalculateJacobian(ekf_.x_);ekf_.UpdateEKF(measurement_pack.raw_measurements_);} else {// Laser updatesekf_.R_ = R_laser_;ekf_.H_ = H_laser_;ekf_.Update(measurement_pack.raw_measurements_);}// print the outputcout << "x_ = " << ekf_.x_ << endl;cout << "P_ = " << ekf_.P_ << endl;
}

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

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

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