# IMU-Aided High-Frequency Lidar Odometry for Autonomous Driving
## 3 The Proposed Approach
### Output Step: Fusion in an EKF Framework  
#### 3.6.1 Basic Concepts  
The Extended Kalman filter (EKF) [28] is one of the most commonly used data fusion algorithms. It is a recursive state estimator for the nonlinear system, and the state is assumed to satisfy a Gaussian distribution. A typical EKF algorithm consists of two steps: a prediction step, and an update step.  
In the prediction step, the previous state \(\mathbf{x}_{\mathrm{k}-1}\) is used to compute a priori estimate of the current state by:  
\[\begin{split}\mathbf{\bar{x}}_{\mathrm{k}}&=\mathbf{f}\left( \mathbf{\hat{x}}_{\mathrm{k}-1}\right)\,,\\ \mathbf{\bar{P}}_{\mathrm{k}}&=\mathbf{F}\,\mathbf{\hat{P}}_{ \mathrm{k}-1}\,\mathbf{F}^{T}+\mathbf{R}\,,\end{split} \tag{16}\]  
where \(R\) is the covariance of the process noise. \(\mathbf{\bar{x}}_{\mathrm{k}}\) and \(\mathbf{\bar{P}}_{\mathrm{k}}\) are the mean and covariance matrices of the prior distribution of the state \(\mathbf{x}_{\mathrm{k}}\), respectively. \(\mathbf{\hat{x}}_{\mathrm{k}-1}\) and \(\mathbf{\hat{P}}_{\mathrm{k}-1}\) are the mean and covariance matrices of the posterior Gaussian distribution of the state \(\mathbf{x}_{\mathrm{k}-1}\). By performing a first-order Taylor expansion to the function \(\mathbf{f}\) at the mean of the state \(\mathbf{x}_{\mathrm{k}-1}\), the first-order partial derivative can be denoted as the Jacobian matrix \(\mathbf{F}\):  
\[\mathbf{F}=\frac{\partial\mathbf{f}}{\partial\mathbf{x}_{\mathrm{k}-1}}\bigg{|}_{\mathbf{\hat {x}}_{\mathrm{k}-1}}\,. \tag{17}\]  
In the update step, the priori estimate of the current state is combined with the current observation \(\mathbf{z}_{\mathrm{k}}\) to refine the state estimate and obtain the posteriori state estimate \(\mathbf{x}_{\mathrm{k}}\), which can be formulated as:  
\[\begin{split}\mathbf{K}_{\mathrm{k}}&=\mathbf{\bar{P}}_{ \mathrm{k}}\,\mathbf{H}^{T}\,\left(\mathbf{H}\,\mathbf{\bar{P}}_{\mathrm{k}}\,\mathbf{H}^{T}+ \mathbf{Q}\right)^{-1}\,,\\ \mathbf{\hat{x}}_{\mathrm{k}}&=\mathbf{\bar{x}}_{\mathrm{k} }+\mathbf{K}_{\mathrm{k}}\left(\mathbf{z}_{\mathrm{k}}-\mathbf{h}\left(\mathbf{\bar{x}}_{ \mathrm{k}}\right)\right)\,,\\ \mathbf{\hat{P}}_{\mathrm{k}}&=\left(\mathbf{I}-\mathbf{K}_{ \mathrm{k}}\mathbf{H}\right)\mathbf{\bar{P}}_{\mathrm{k}}\,,\end{split} \tag{18}\]  
where \(\mathbf{Q}\) represents the covariance of the measurement noise. \(\mathbf{K}_{\mathrm{k}}\) is the Kalman gain and the posteriori state estimation \(\mathbf{x}_{\mathrm{k}}\sim\mathcal{N}\left(\mathbf{\hat{x}}_{\mathrm{k}},\mathbf{\hat{P}}_ {\mathrm{k}}\right)\). Similarly, a first-order Taylor expansion to the function \(\mathbf{h}\) can be performed at the mean of the prior distribution of the state \(\mathbf{x}_{\mathrm{k}}\). The first-order partial derivative could be defined as the Jacobian matrix \(\mathbf{H}\):  
\[\mathbf{H}=\frac{\partial\mathbf{h}}{\partial\mathbf{x}_{\mathrm{k}}}\bigg{|}_{\mathbf{\hat{x} }_{\mathrm{k}}}\,. \tag{19}\]  
In this section, We use an EKF framework to fuse the IMU and the results obtained from the lidar odometry, as shown in Figure 5. Let the motion prior model presented in Section 3.2 be the system model and the results from the lidar odometry (which is denoted as the red star in Figure 5) be the measurements. The system model and measurement model are described as follows.