# IMU-Aided High-Frequency Lidar Odometry for Autonomous Driving
## 3 The Proposed Approach
### Output Step: Fusion in an EKF Framework
#### 3.6.3 Measurement Model  
The measurement vector \(\mathbf{z}_{\mathrm{k}}\) is denoted as:  
\[\mathbf{z}_{\mathrm{k}}= \begin{bmatrix}\theta_{z_{\mathrm{k}}}^{x}&\theta_{z_{ \mathrm{k}}}^{y}&\theta_{z_{\mathrm{k}}}^{z}&p_{z_{\mathrm{k}}}^{x}&p_{z_{ \mathrm{k}}}^{z}&p_{z_{\mathrm{k}}}^{z}\end{bmatrix} ^{T}\enspace. \tag{28}\]  
According to Figure 5, the lidar odometry algorithm starts to run at time \(t_{\mathrm{m}}^{\mathrm{L}}\), but the result is obtained at time \(t_{\mathrm{k}}\), which is several tens of milliseconds after \(t_{\mathrm{m}}^{\mathrm{L}}\). We assume that the pose of the vehicle at time \(t_{\mathrm{m}}^{\mathrm{L}}\) and \(t_{\mathrm{k}}\) were approximately equal to \(\mathbf{T}_{\mathrm{i}}^{\mathrm{WB}}\) and \(\mathbf{T}_{l}^{\mathrm{WB}}\). Both \(\mathbf{T}_{\mathrm{i}}^{\mathrm{WB}}\) and \(\mathbf{T}_{l}^{\mathrm{WB}}\) can be obtained from the IMU data. The relationship between the measurement \(\mathbf{z}_{\mathrm{k}}\) and the real state \(\mathbf{x}_{\mathrm{k}}\) of the vehicle at time \(t_{\mathrm{k}}\) can be indicated by \(\triangle\mathbf{T}_{\mathrm{i},l}^{\mathrm{B}}\), which can also be denoted as \(\triangle\mathbf{\xi}_{\mathrm{i},l}= \begin{bmatrix}\triangle\mathbf{\theta}_{ \mathrm{i},l}&\triangle\mathbf{p}_{\mathrm{i},l}\end{bmatrix} ^{T}\). The observation equation could then be represented as:  
\[\mathbf{z}_{\mathrm{k}}= \begin{bmatrix}\theta_{\mathrm{k}}\\ \triangle\mathbf{p}_{\mathrm{i},l}\end{bmatrix} + \begin{bmatrix}\mathbf{J}_{l}^{-1} \left(\theta_{\mathrm{k}}\right)\cdot\triangle\mathbf{\theta}_{\mathrm{i},l}\\ \triangle\mathbf{R}_{\mathrm{i},l}^{\mathrm{B}}\cdot\mathbf{p}_{\mathrm{k}}\end{bmatrix} \enspace. \tag{29}\]  
According to Equations (19) and (29), \(\mathbf{H}\) can be derived as:  
\[\mathbf{H}= \begin{bmatrix}\mathbf{H}_{\theta\theta}&\mathbf{0}_{3\times 3}\\ \mathbf{0}_{3\times 3}&\mathbf{H}_{pp}\end{bmatrix} \enspace, \tag{30}\]  
where \(\mathbf{H}_{pp}=\triangle\mathbf{R}_{\mathrm{i},l}^{\mathrm{WB}}\in\mathbb{R}^{3\times 3}\), \(\mathbf{H}_{\theta\theta}\in\mathbb{R}^{3\times 3}\) and \(\mathbf{H}_{\theta\theta}\) can be computed by:  
\[\mathbf{H}_{\theta\theta}=\begin{bmatrix}\left(\frac{\partial J_{l}^{-1}(\theta_{ \mathrm{k}})}{\partial\theta_{\mathrm{k}}^{2}}\cdot\triangle\mathbf{\theta}_{ \mathrm{i},l}\right)^{T```
input :The history pose set \(\{T_{\rm i}^{\rm WB}\}\) and pose \(T_{\rm B_{k}}^{\rm WB}\) at time \(t_{\rm k}^{\rm B}\) from the IMU; pose \(T_{\rm L_{k}}^{\rm WB}\) at time \(t_{\rm k}^{\rm L}\) from the lidar odometry; previous pose \(T_{\rm k-1}^{\rm WB}\) output :Current pose \(T_{\rm k}^{\rm WB}\)
1 begin
2 use \(T_{\rm B_{k}}^{\rm WB}\) and \(T_{\rm k-1}^{\rm WB}\) to do the state transition by Equation (16) and obtain the prior distribution \(\tilde{x}_{\rm k}\);
3 ifhave\(T_{\rm L_{k}}^{\rm WB}\)then
4 use \(\tilde{x}_{\rm k}\), \(T_{\rm L_{k}}^{\rm WB}\) and \(\{T_{\rm i}^{\rm WB}\}\) to compute the posterior distribution \(\hat{x}_{\rm k}\) by Equation (18) output the \(T_{\rm k}^{\rm WB}\) by \(\hat{x}_{\rm k}\)
5 end if
6 else
7 output the \(T_{\rm k}^{\rm WB}\) by \(\tilde{x}_{\rm k}\)
8 end if
9
10 end for
```

**Algorithm 1**The fusion framework of our system. ## 4 Experimental Results

The platform used in the following experiments is shown in Figure 6. The vehicle is equipped with a Velodyne HDL-64E lidar, a Xsens MTI300 IMU, two photoelectric encoders, a NovAtel SPAN-CPT GNSS/INS system, and a Nova 5100 industrial computer. The lidar is mounted on the top of the car, and it has 64 beams which could generate over 1.3 million points per second with a high range accuracy. The horizontal Field of View (FOV) is 360\({}^{\circ}\), the vertical FOV is 26.8\({}^{\circ}\), and the maximal detection distance is 120 m. The NovAtel GNSS/INS system provides localization results with centimeter-level accuracy, which can be served as the ground truth.