本系列的前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$，方向夾角 $\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 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_ */

初始化時，需要初始化狀態向量$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=?\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}?$$
假設障礙物做勻速運動，則在經過$\delta t$時間后，狀態向量為：
$$x^{\prime }=?\begin{array}{c}x+v_x?\delta t\\ y+v_y?\delta t\\ v_x\\ v_y\end{array}?$$
如果用矩陣表示的話，則狀態轉移公式為：
$$?\begin{array}{c}x+v_x?\delta t\\ y+v_y?\delta t\\ v_x\\ v_y\end{array}?=?\begin{array}{cccc}1& 0& \delta t& 0\\ 0& 1& 0& \delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}???\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}?$$
因此，狀態轉移矩陣為$(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 Pekf_.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 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?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]??\begin{array}{c}x+v_x?\delta t\\ y+v_y?\delta t\\ v_x\\ v_y\end{array}?$$
因此這里的測量矩陣$H$為：

// Lidar - measurement matrixH_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 - laserR_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 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）預測

這里再回顧下卡爾曼濾波預測中的兩個公式：
$$\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=?\begin{array}{c}\rho \\ \\ \phi \\ \\ \dot{\rho }\end{array}???\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}?$$

其中，${\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 }=?\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}?=?\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}???\begin{array}{c}{\rho }_x\\ {\rho }_y\\ v_x\\ v_y\end{array}?$$
顯然，上式兩邊轉化為非線性轉換，那么如何才能將非線性函數用近似線性函數表達呢？

這里我們使用泰勒公式來近似，近似后的泰勒公式為：
$$h(x)\approx h(x_0)+\frac{?h(x_0)}{?x}(x?x_0)$$
這里忽略了二次以上的高階項。對$x$求導后的項為雅克比公式。這里，雅克比公式就是我們的測量矩陣$H$。

最終測量矩陣為：
$$H=?\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}?$$
這里對測量矩陣進行初始化$H$：

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

測量噪聲協方差矩陣$R$為：

//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;}

最后還有兩個公式：
$$\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 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傳感器，在下一篇博客中將會進行介紹。

創作挑戰賽新人創作獎勵來咯，堅持創作打卡瓜分現金大獎

總結

以上是生活随笔為你收集整理的动手学无人驾驶（5）：多传感器数据融合的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。