LiDAR–inertial estimation course › Lesson 9 › Reference card

Reference card · Lesson 9

The tight solve

Trace a scan through prediction, association, joint optimization, and map update.

For the local Gaussian approximation, \(P\approx H_{\rm total}^{-1}\); take marginal covariance blocks after solving the full system. Priors are already included in \(H_{\rm total}\). See the derivation and implementation limits.

Key equations

Warm-up, from memory

$$ \mathbf{r}_{\Delta v} = \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij}) - \Delta\hat{\mathbf{v}}_{ij}(\mathbf{b}) $$

General joint objective

$$ \mathcal{X}^{\star}=\arg\min_{\mathcal{X}}\; \|\mathbf{r}_{\mathrm{prior}}\|^2_{\Sigma_{\mathrm{prior}}} +\sum_{ij}\|\mathbf{r}_{\mathrm{imu},ij}\|^2_{\Sigma_{\mathrm{imu},ij}} +\sum_l\rho\!\left(r_{\mathrm{lidar},l}/\sigma_l\right). $$

One current-state implementation

$$ \begin{aligned} \arg\min_{\mathbf{x}_j,\,\mathbf{g}}\;\; &\sum_{l} \rho\big(r_l/\sigma\big) \;+\; \|\mathbf{r}_{\mathcal{I}}\|^{2}_{\Sigma_{\text{eff}}}\\ +\;&\|\mathbf{b}_j - \mathbf{b}_i\|^{2}_{\mathbf{P}_b} \;+\; \|\mathbf{g} - \mathbf{g}_{\text{init}}\|^{2}_{\sigma_g^{2}\mathbf{I}} \end{aligned} $$

Explain the idea

Explain the idea: one scan

Put LiDAR and IMU measurements on a consistent time interval and in consistent frames. Use the IMU to predict motion, associate LiDAR points with map geometry, and combine their residuals with priors using justified uncertainty weights. Iterate the state update when the problem is nonlinear, then check whether the measurements constrained the answer. Carry an appropriate uncertainty summary forward and add the scan to the map only when its pose is trusted. The exact state, solver, gates, and feedback rules depend on the estimator.