# IMU-Aided High-Frequency Lidar Odometry for Autonomous Driving
## 3 The Proposed Approach
### Motion Prior from IMU and Wheel Encoder  
It is well-known that the accelerometer in the IMU is affected by the gravity, and it needs to be integrated twice to obtain the translational vector [20]. In contrast, the gyro in the IMU directly measures the angular velocity, and it only needs to be integrated once to obtain the rotation angle. It is reasonable to believe that the angular output from the IMU is much more accurate than the translational measurement. For the ground vehicle, besides IMU, it is also equipped with the wheel encoder. The wheel encoder directly measures the rotation of the wheel, and thus provides a much more accurate translational measurement. In this paper, we try to combine the angular output from the IMU and the translational output from the wheel encoder to obtain the motion prior. The motion model of the vehicle can be simplified as Figure 4. We mount the IMU at the center of two rear wheels, and mount two wheel encoders on the left and right rear wheels, respectively. The black car denotes the position of the vehicle at the previous moment \(t^{\text{B}}_{\text{k}-1}\), and the blue car indicates its position at the current time \(t^{\text{B}}_{\text{k}}\). \(\triangle sl\) and \(\triangle sr\) represent the traveled distances of the left rear wheel and the right rear wheel during the short period of time. Firstly, we compute the rotation matrix \(\mathbf{R}^{\text{WB}}_{\text{k}}\) by triaxial angular velocities of the IMU. We introduce the following kinematic model [21]:  
\[\frac{\text{d}\mathbf{R}^{\text{BW}}_{\text{r}}}{\text{d}t}=\mathbf{R}^{\text{BW}}_{ \text{r}} \begin{bmatrix}\omega^{x}_{\text{r}}\\ \omega^{y}_{\text{r}}\\ \omega^{z}_{\text{r}}\end{bmatrix} ^{\wedge}. \tag{5}\]  
In Equation (5), \(\mathbf{R}^{\text{BW}}_{\text{r}}\) indicates the rotation matrix from the coordinate system \(\{B\}\) to \(\{W\}\). \(\omega^{x}_{\text{r}\tau}\)\(\omega^{y}_{\text{r}}\), and \(\omega^{z}_{\text{r}}\) represent the instantaneous angular velocities of the IMU. By assuming the angular velocity of the IMU constant during \([t_{\rm k-1}^{\rm B},t_{\rm k}^{\rm B}]\), we integrate Equation (5) from \(t_{\rm k-1}^{\rm B}\) to \(t_{\rm k}^{\rm B}\) and solve the differential equation. The rotation matrix \(\mathbf{R}_{\rm k}^{\rm BW}\) can be obtained by:  
\[\mathbf{R}_{\rm k}^{\rm BW}=\mathbf{R}_{\rm k-1}^{\rm BW}\,{\rm Exp}\,\Big{(}\,(\mathbf{ \omega}_{\rm k}\,\triangle t)^{\wedge}\,\Big{)}\,. \tag{6}\]  
To compute the translational component \(\mathbf{p}_{\rm k}^{\rm WB}\) of the pose \(\mathbf{T}_{\rm k}^{\rm WB}\), we approximate the traveled distance \(\triangle s\) of the vehicle to be the average of the two rear wheels. \(\triangle s\) indicates the relative displacement of the vehicle during \([t_{\rm k-1}^{\rm B},t_{\rm k}^{\rm B}]\) in the body coordinate system \(\{B\}\) and the instantaneous speed of the vehicle can be denoted as \(v=\triangle s/(t_{\rm k}^{\rm B}-t_{\rm k-1}^{\rm B})\). We assume the velocity of the vehicle to be constant during \([t_{\rm k-1}^{\rm B},t_{\rm k}^{\rm B}]\), where the wheel encoder always records the distance traveled along the heading direction of the vehicle. Thus, the instantaneous velocity vector can be denoted as \(v_{\rm k}^{\rm B}=[v\ \ 0\ \ 0]^{\rm T}\).