# IMU-Aided High-Frequency Lidar Odometry for Autonomous Driving
## 3 The Proposed Approach
### Output Step: Fusion in an EKF Framework
#### 3.6.2 System Model  
Since the main purpose of our system is to obtain the pose of the vehicle, we could use the _Lie Algebra_\(\xi\) to define the state vector \(\mathbf{x}_{\text{k}}\) at time \(t_{\text{k}}\) as:  
\[\mathbf{x}_{\text{k}}= \begin{bmatrix}\theta_{\text{k}}&\mathbf{p}_{\text{k}}\end{bmatrix}  ^{T}\in\mathfrak{se}(3)\,. \tag{20}\]  
By utilizing the logarithmic map and the approximate Baker-Campbell-Hausdorff (BCH) formula [21], the \(\mathbf{R}_{\text{k}}^{\text{WB}}\) obtained from the Equation (6) can be converted to:  
\[\mathbf{\theta}_{\text{k}}=\mathbf{J}_{l}^{-1}\left(\mathbf{\theta}_{\text{k}-1}\right) \cdot\left(\mathbf{\omega}_{\text{k}}\triangle t\right)+\mathbf{\theta}_{\text{k}-1}\,, \tag{21}\]  
where \(\mathbf{J}_{l}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)\) denotes the right BCH Jacobian of \(\text{SO}(3)\) and it equals to:  
\[\mathbf{J}_{l}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)=\frac{\theta_{\text{k}}}{ 2}\text{cot}\frac{\theta_{\text{k}}}{2}\mathbf{I}+\left(1-\frac{\theta_{\text{k}} }{2}\text{cot}\frac{\theta_{\text{k}}}{2}\right)\mathbf{n}_{\text{k}}\mathbf{n}_{ \text{k}}^{T}-\frac{\theta_{\text{k}}}{2}\mathbf{n}_{\text{k}}^{\wedge}\,, \tag{22}\]  
and \(\mathbf{J}_{l}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)\) can be denoted as:  
\[\mathbf{J}_{l}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)= \begin{bmatrix}J_{1}^{-1} \left(\mathbf{\theta}_{\text{k}}\right)&J_{2}^{-1}\left(\mathbf{\theta}_{\text{k}} \right)&J_{3}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)\\ J_{4}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)&J_{5}^{-1}\left(\mathbf{\theta}_{ \text{k}}\right)&J_{6}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)\\ J_{7}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)&J_{8}^{-1}\left(\mathbf{\theta}_{ \text{k}}\right)&J_{9}^{-1}\left(\mathbf{\theta}_{\text{k}}\right)\end{bmatrix} \,. \tag{23}\]  
Combining the Equations (8), (21) and (23), the state transition equation could be defined as:  
\[\mathbf{x}_{\text{k}}=\mathbf{x}_{\text{k}-1}+ \begin{bmatrix}J_{l}^{-1}\left(\mathbf{ \theta}_{\text{k}-1}\right)\cdot\mathbf{\omega}_{\text{k}}\triangle t\\ \mathbf{R}_{\text{k}}^{\text{WB}}(1:3,1)\cdot\triangle s\end{bmatrix} \,, \tag{24}\]  
where \(\mathbf{R}_{\text{k}}^{\text{WB}}\) is computed by \(\theta_{\text{k}}^{x}\), \(\theta_{\text{k}}^{y}\) and \(\theta_{\text{k}}^{z}\) according to Equation (3) and \(\mathbf{R}_{\text{k}}^{\text{WB}}(1:3,1)\) means the first column of the \(\mathbf{R}_{\text{k}}^{\text{WB}}\). According to Equations (17) and (24), \(\mathbf{F}\) can be derived as:  
\[\mathbf{F}= \begin{bmatrix}\mathbf{F}_{\theta\theta}&\mathbf{0}_{3\times 3}\\ \mathbf{F}_{p\theta}&\mathbf{I}_{3\times 3}\end{bmatrix} \in\mathbb{R}^{6\times 6}\,, \tag{25}\]  
where \(\mathbf{F}_{\theta\theta}\in\mathbb{R}^{3\times 3}\), and it can be computed by:  
Figure 5: The diagram of the proposed fusion framework. The yellow rectangle represents the IMU data, and the blue triangle indicates the lidar point clouds. The red star denotes the localization result output from the lidar odometry. \[\mathbf{F}_{\theta\theta}=\mathbf{I}_{3\times 3}+ \begin{bmatrix}\left(\frac{\partial J_{l}^{-1}( \theta_{\mathrm{k-1}})}{\partial\theta_{\mathrm{k-1}}^{2}}\cdot\mathbf{\omega}_{ \mathrm{k}}\triangle t\right)^{T}\\ \left(\frac{\partial J_{l}^{-1}(\theta_{\mathrm{k-1}})}{\partial\theta_{ \mathrm{k-1}}^{2}}\cdot\mathbf{\omega}_{\mathrm{k}}\triangle t\right)^{T}\\ \left(\frac{\partial J_{l}^{-1}(\theta_{\mathrm{k-1}})}{\partial\theta_{ \mathrm{k-1}}^{2}}\cdot\mathbf{\omega}_{\mathrm{k}}\triangle t\right)^{T}\end{bmatrix}  \enspace. \tag{26}\]  
\(\mathbf{F}_{p\theta}\) can be computed by:  
\[\mathbf{F}_{p\theta}= \begin{bmatrix}\frac{\partial R_{\mathrm{k}}^{\mathrm{WB}}(1,3,1)}{\partial R_{\mathrm{k-1}}^{2}}&\frac{\partial R_{\mathrm{k}}^{\mathrm{WB }}(1,3,1)}{\partial\theta_{\mathrm{k-1}}^{2}}&\frac{\partial R_{\mathrm{k}}^{ \mathrm{WB}}(13,1)}{\partial\theta_{\mathrm{k-1}}^{2}}\end{bmatrix} \enspace.