## 3 Multi-Sensor Fusion
### Tight Coupling of Dual Lidar and IMU
#### 3.4.2 Joint Optimization  
In this paper, a fixed-lag smoother and marginalization are introduced to obtain the optimal state. The fixed-lag smoother maintains a certain amount of IMU state in the sliding window, and the sliding window can effectively control the amount of calculation. When the new state enters the sliding window, the past state is marginalized. The state variable to be estimated for the whole window is \(\mathbf{X}=[\mathbf{X}_{\mathbf{q}}^{\mathbf{W}},\ldots,\mathbf{X}_{\mathbf{k}}^{\mathbf{W}}, \mathbb{Z}_{\mathbf{I}}^{\mathbf{L}_{1}}]\), where \(\mathbf{X}_{\mathbf{q}}^{\mathbf{W}}\) is the state of IMU at the starting point \(\mathbf{\eta}\) of the sliding window; \(\mathbf{X}_{\mathbf{k}}^{\mathbf{W}}\) is the state of IMU at the end of the sliding window; and \(\mathbb{Z}_{\mathbf{I}}^{\mathbf{L}_{1}}=[\mathbb{R}_{\mathbf{I}}^{\mathbf{L}_{1}},\mathbf{H}_\mathbf{{I}}^{\mathbf{L}_{1}}]\) is the external parameter between lidar and IMU. Then, the following cost function with a Mahalanobis norm is minimized to obtain the MAP estimation of the states \(\mathbf{X}\),  
\[\mathbf{X}=\min_{\mathbf{X}}\frac{1}{2}\Bigg{\{}\left\|\mathbf{u}_{\mathbf{o}}(\mathbf{X})\right\| ^{2}+\sum_{\mathbf{X}\in\Omega_{\mathbf{I}_{\mathbf{\beta}}}}\left\|\mathbf{u}_{\mathbf{\phi}}( \mathbf{\chi},\mathbf{X})\right\|_{\mathcal{C}_{\mathbf{\beta}}}^{2}+\sum_{\mathbf{\xi}\in\  
where \(\mathbf{u}_{\mathbf{o}}(\mathbf{X})\) is the prior information from marginalization [26]. \(\mathbf{u}_{\mathbf{\phi}}(\mathbf{\chi},\mathbf{X})\) is the residual of the relative lidar constraints that can be represented as point-to-plane distance [26], where \(\mathbf{\chi}\in\mathbf{Q}_{\mathbf{\beta}}\) is the residual for each relative lidar measurement with the previous correspondences. \(\mathbf{\beta}\in\{\mathbf{\eta}+1,\ldots,\mathbf{\kappa}\}\), \(\mathbf{\eta}+1\) and \(\mathbf{\kappa}\) are the timestamps of the lidar sweep next to the starting one and the current lidar sweep in the window, with the covariance matrix \(\mathbf{C}_{\mathbf{L}_{\mathbf{\beta}}}^{\mathbf{X}}\) determined by the lidar accuracy [35]. \(\mathbf{u}_{\gamma}(\mathbf{Z}_{\xi+1}^{\xi},\mathbf{X})\) is the residual of the IMU constraints, where  
\[\mathbf{u}_{\gamma}(\mathbf{Z}_{\xi+1}^{\xi},\mathbf{X})=\left[\begin{array}{c}\mathbf{R}_{ \xi}^{\mathbf{T}}\Big{(}\mathbf{p}_{\xi+1}-\mathbf{p}_{\xi}-\mathbf{v}_{\xi}\Delta\mathbf{t}- \frac{1}{2}\mathbf{g}\Delta\mathbf{t}_{\xi\xi+1}^{2}\Big{)}-\Delta\mathbf{p}_{\xi\xi+1}\\ \mathbf{R}_{\xi}^{\mathbf{T}}\big{(}\mathbf{v}_{\xi+1}-\mathbf{v}_{\xi}-\mathbf{g}\Delta\mathbf{t}_{ \xi\xi+1}\big{)}-\Delta\mathbf{v}_{\xi\xi+1}\\ 2\Big{[}\Delta\mathbf{q}_{\xi\xi+1}^{-1}\otimes\mathbf{q}_{\xi}^{-1}\otimes\mathbf{q}_{ \xi+1}\Big{]}_{\mathbf{x}\mathbf{y}\mathbf{z}}\\ \mathbf{b}_{\xi+1}^{\mathbf{a}}-\mathbf{b}_{\xi}^{\mathbf{a}}\\ \mathbf{b}_{\xi+1}^{\mathbf{c}}-\mathbf{b}_{\xi}^{\mathbf{\omega}}\end{array}\right] \tag{9}\]  
\(\mathbf{u}_{\gamma}(\mathbf{Z}_{\xi+1}^{\xi},\mathbf{X})\) can be obtained by IMU state prediction (Equation (2)) and IMU pre-integration (Equation (3)), and \(\Big{[}\Delta\mathbf{q}_{\xi\xi+1}^{-1}\otimes\mathbf{q}_{\xi}^{-1}\otimes\mathbf{q}_{ \xi+1}\Big{]}_{\mathbf{x}\mathbf{y}\mathbf{z}}\) stands for the vector part of a quaternion. With the continuous-time linearized propagation of the error states and the IMU noise parameters, the covariances \(\mathbf{C}_{\mathbf{L}_{\xi+1}}^{\mathbf{I}_{\xi}}\) of the pre-integration measurements and biases can be estimated. The cost function, in the form of a non-linear least-square, can be solved by the Gauss-Newton algorithm. Ceres Solver [36] is used for solving this nonlinear problem.  
It can be seen from Section 3.2 that the new states \(\mathbf{X}\) are obtained by the joint optimization, which is used as the next state of the IMU, avoiding the drift from IMU propagation. The state of the IMU is applied to deskewing, thereby eliminating the motion distortion of the dual lidar.