1 - Loosely vs Tightly Coupled LIO
LiDAR-inertial odometry combines LiDAR geometry with IMU motion prediction. At a high level, there are two common ways to do this: loosely coupled fusion and tightly coupled fusion.
In a loosely coupled system, each sensor module is mostly solved independently. For example, the IMU may propagate an ESKF state, while a LiDAR odometry module independently runs ICP, NDT, LOAM-style optimization, or another scan-matching method. The LiDAR module then outputs a pose estimate, which is fused back into the filter as a high-level measurement.
In a tightly coupled system, the estimator does not wait for LiDAR to produce a complete pose solution. Instead, LiDAR residuals are inserted directly into the estimator together with the IMU prediction. The optimizer or filter jointly uses IMU state uncertainty and individual LiDAR residuals when computing the correction.
You can think of loosely coupled LIO as a design with relatively clear module boundaries, or “high cohesion, low coupling” (高内聚，低耦合): the lidar odometry front-end solves its own problem first, and the IMU or INS is fused afterward. That separation is clean from a system-design perspective, but it can make the lidar front-end more exposed to geometric degeneracy. The difference may look subtle when all sensors work well. But when LiDAR becomes geometrically weak, the two designs behave very differently.
2 - IEKF: Iterated Error-State Kalman Filter
IMU gives a drifting prior, and NDT gives a geometric pull-back-to-map constraint. IEKF fuses them by solving a small weighted least-squares problem on the error state, then relinearizing and solving again.
Error state:
\[\delta x = [\delta p, \delta v, \delta \theta, \delta b_g, \delta b_a, \delta g]\]Nominal state:
\[x = [p, v, \theta, b_g, b_a, g]\]3 - Prediction with IMU
3 - 1 Contiunous State Transition
The IMU measurements are
\(\omega_m=\omega+b_g+n_g,\) \(a_m=R_{bw}(a_w-g)+b_a+n_a.\)
Therefore, using the current bias estimates,
\(\hat\omega=\omega_m-\hat b_g,\)
$$
\hat a_w =
R_{wb}(a_m-\hat b_a)+\hat g.
$$
The nominal state is propagated as
$$
p_{k+1} =
p_k+v_k\Delta t+\frac12\hat a_w\Delta t^2,
\(\)
v_{k+1} =
v_k+\hat a_w\Delta t,
\(\)
R_{k+1} =
R_k\operatorname{Exp}
\left(
[\hat\omega]_\times\Delta t
\right).
$$
The biases and gravity are kept constant during nominal propagation:
Their uncertainty still grows through the process noise.
Use the error state
\[\delta x= \begin{bmatrix} \delta p& \delta v& \delta\theta& \delta b_g& \delta b_a& \delta g \end{bmatrix}^{\top}.\]For a right rotation error, the continuous-time linearized dynamics are approximately
\(\delta\dot p=\delta v,\)
$$
\delta\dot v =
-R[a_m-\hat b_a]_\times\delta\theta
-R\delta b_a
+\delta g
-Rn_a,
\(\)
\delta\dot\theta =
-[\omega_m-\hat b_g]\times\delta\theta
-\delta b_g
-n_g,
\(\)
\delta\dot b_g=n{bg},
\qquad
\delta\dot b_a=n_{ba},
\qquad
\delta\dot g=0.
$$
3 - 2 Discrete State Transition
After discretization,
\[\delta x_{k+1} = F_k\delta x_k+G_dw_k.\]Using a first-order discretization,
\[F_k \approx \begin{bmatrix} I&I\Delta t&0&0&0&0\\ 0&I&-R[\hat a_b]_\times\Delta t&0&-R\Delta t&I\Delta t\\ 0&0&\operatorname{Exp}(-[\hat\omega]_\times\Delta t)&-I\Delta t&0&0\\ 0&0&0&I&0&0\\ 0&0&0&0&I&0\\ 0&0&0&0&0&I \end{bmatrix},\]where
\[\hat a_b=a_m-\hat b_a.\]The ($-I\Delta t$) gyro-bias block is a first-order approximation. A more accurate discretization uses the (SO(3)) right Jacobian.
3 - 3 Covariance propagation
The predicted covariance is
\[\boxed{ P_{k+1}^{-} = F_kP_k^{+} F_k^\top + G_dQ_wG_d^\top }\]where ($Q_w$) contains the gyro noise, accelerometer noise, and bias random-walk covariances. If the noise mapping has already been included, define
\[Q_d=G_dQ_wG_d^\top\]and write
\[\boxed{ P_{k+1}^{-} = F_kP_k^{+} F_k^\top+Q_d. }\]The identity blocks for ($b_g$), ($b_a$), and ($g$) mean that these errors are constant during deterministic propagation. They can still be corrected during the LiDAR measurement update.
4 - Observation Update with NDT
Different papers use different symbols. Here, we use:
| EKF notation | NDT notation | Meaning |
|---|---|---|
| H or C | J | Stacked measurement Jacobian |
| V or R | $\Sigma$ | Stacked measurement covariance |
| r | e | Stacked residual vector |
| $H^\top V^{-1}H$ | $J^\top\Sigma^{-1}J$ | Measurement information matrix |
| $-H^\top V^{-1}r$ | $-J^\top\Sigma^{-1}e$ | Residual gradient term |
4 - 1 NDT residual
For a source point $p_i$, let $\mu_i$ and $\Sigma_i$ be the mean and covariance of its matched NDT voxel.
The transformed point is
\[p_i^w = Rp_i+t.\]The residual is
\[e_i = Rp_i+t-\mu_i.\]Its weighted cost is
\[\chi_i^2=e_i^\top\Sigma_i^{-1}e_i.\]Here, ($e_i$) is a vector. The value ($\chi_i^2$) is a scalar cost.
4 - 2 Residual Jacobian
Assuming a right rotation perturbation,
\[R_{\text{new}}=R\operatorname{Exp}(\delta\theta),\]the linearized residual is
\[e_i(\delta x)\approx e_i+J_i\delta x.\]The position and rotation blocks are
\[\frac{\partial e_i}{\partial \delta p}=I, \qquad \frac{\partial e_i}{\partial \delta\theta} -R[p_i]_\times.\]For the state ordering
\[\delta x= \begin{bmatrix} \delta p& \delta v& \delta\theta& \delta b_g& \delta b_a& \delta g \end{bmatrix}^\top,\]the full Jacobian is
\[J_i= \begin{bmatrix} I& 0& -R[p_i]_\times& 0& 0& 0 \end{bmatrix}.\]4.3 Iterated State Update and One-shot Covariance Update
Let
-
$\hat x^-$ be the propagated IMU state;
-
$\bar P$ be its covariance;
-
$\hat x_k$ be the nominal state at iteration $k$.
At each iteration, NDT computes the residual and Jacobian at the current state:
\[-e_k \approx J_k \delta x_k +v.\]The IEKF then:
-
Recomputes NDT correspondences, residuals, and Jacobians.
-
Solves for the correction $\delta x_k$.
-
Injects the correction into the nominal state.
-
Repeats until the correction is small.
The same propagated IMU prior is used throughout these iterations because they all process the same LiDAR scan.
4-3-1 First iteration
At the first iteration,
\[\hat x^0=\hat x^-.\]Therefore, the prior error has zero mean, and the optimization is
\[\delta x^* = \arg\min_{\delta x} \left[ \lVert\delta x\rVert_{\bar P^{-1}}^2 + \lVert e+J\delta x\rVert_{\Sigma^{-1}}^2 \right].\]This gives
\[\boxed{ \delta x = K(z-h) = \left( \bar P^{-1} + J^\top\Sigma^{-1}J \right)^{-1} \left( -J^\top\Sigma^{-1}e \right). }\]Define
\[A=J^\top\Sigma^{-1}J, \qquad b=-J^\top\Sigma^{-1}e.\]Then
\[\boxed{ \delta x=(\bar P^{-1}+A)^{-1}b. }\]4-3-2 Later iterations
After the first iteration,
\[\hat x_k\neq\hat x^-.\]Let $x_0$ be the original propagated IMU state and ($x_k$) the state at IEKF iteration (k). Define $d_k$ to be delta x w.r.t $x_0$:
\[d_k=x_k\boxminus x_0.\]Now consider a small perturbation $\delta x_{k,\text{raw}}$ around $x_k$. Expressed back in the tangent space of $x_0$, we accumulate it:
\[d_{k+1} \approx d_k+T_k\delta x_{k,\text{raw}},\]where $T_k$ is essentially the local Jacobian that maps perturbations from the tangent space at $x_k$ back to the tangent space at $x_0$.
\[\boxed{ T_k = \frac{\partial(\text{perturbation expressed at }x_0)} {\partial(\text{perturbation expressed at }x_k)}. }\]The correct objective is therefore
\[\boxed{ \delta x_{k} ^* = \arg\min_{\delta x} \left[ \left| d_k+T_k\delta x \right|_{\bar P^{-1}}^2 + \left| e_k+J_k\delta x \right|_{\Sigma^{-1}}^2 \right]. }\]The normal equation is
\[\begin{aligned} \Big[ (T_k)^\top\bar P^{-1}T_k + (J_k)^\top\Sigma^{-1}J_k \Big]\delta x_k ={}& -(T_k)^\top\bar P^{-1}d_k\ -(J_k)^\top\Sigma^{-1}e_k. \end{aligned}\]Define
\[P_k = (T_k)^{-1} \bar P (T_k)^{-\top},\]and the prior mean in the current tangent space:
\[\mu_k = -(T_k)^{-1}d_k.\]Then the above becomes:
\[\boxed{ \delta x_k = \left[ (P_k)^{-1}+A_k \right]^{-1} \left[ (P_k)^{-1}\mu_k+b_k \right]. }\]During the first iteration,
\[T_0=I, \qquad d_0=0, \qquad \mu_0=0,\]so this reduces to the simpler equation derived earlier.
FAST-LIO2 expresses the same idea by transporting the prior covariance into the current tangent space and including the displacement between the current iterate and the propagated state in every correction. (ar5iv)
Note, $T_k$ is a Jacobian that relates the tangent space of the current nominal state, $\hat x_k$ and the tangent space of the propagated IMU state $\hat x^-$. For position, velocity, biases, and gravity, the state change is additive. so the projection of the state change is identity. Rotation is a bit more complicated, I haven’t fully gotten that yet, but this looks like a right Jacobian using BCH:
\[\boxed{ T_k = \operatorname{diag} \left( I,, I,, J_r^{-1}(\phi_k),, I,, I,, I \right). }\]4-3-3 Covariance Update In One Shot
For a simple nonlinear function
\(y=f(x),\) linearized around $x_0$,
\[y \approx f(x_0)+f'(x_0)\delta x.\]If (x) has covariance $P_x$, then the covariance of $y$ is approximately
\[\boxed{ P_y=f'(x_0)P_xf'(x_0)^\top. }\]The same idea applies in the IEKF, except that the state may lie on a $SO(3)$ manifold .
The propagated covariance $\bar P$ is the covariance matrix at $x_0$. To express the same uncertainty around $x_k$, we need the inverse mapping:
\[\boxed{ P_k = T_*^{-1}\bar P T_*^{-\top}. }\]This step only changes the coordinates of the prior covariance. It does not add measurement information.
Finally, the LiDAR/NDT measurement causes the actual Kalman covariance reduction. Let ($P_k$) be the propagated covariance transported into the tangent space of the converged state. Define
\[A_k = (J_k)^\top \Sigma^{-1} J_k.\]The posterior covariance is
\[\boxed{ P^+ = \left[ (P_k)^{-1}+A_k \right]^{-1}. }\]Equivalently (Rico: you can plugin k and realize that below is the same as the above), this is the final covariance
\[\boxed{ P^+=(I-K_* J_*)P_*. }\]So the overall flow is simply
\[\boxed{ \bar P \overset{\text{iterations}}{\longrightarrow} P_* \overset{\text{ekf fusion}}{\longrightarrow} P^+. }\]FAST-LIO2 similarly iterates the state until convergence and then computes the final covariance from the final gain, measurement Jacobian, and transported prior covariance. (ar5iv)
If the covariance is not already expressed in the tangent space of the final injected state, apply the required reset or tangent-space transport afterward.
5 - Where TIghtly Coupeld LIO Shines - Gauge Freedom
In lidar-inertial odometry, gauge freedom refers to directions in the state space that are weakly observed or unobservable from the current measurements. In practice, this appears as degeneracy: different state updates produce almost the same residual error.
- If the lidar scene is feature-poor or highly repetitive, the lidar-only update can become unreliable.
- The IMU may still provide useful short-term motion propagation, but it does not directly rescue the front-end scan-matching optimization if that optimization is solved independently.
- For this reason, many systems include a degeneracy detection step. When the lidar update becomes weak, the estimator can reduce the correction, keep propagating with the inertial solution, or temporarily fall back to an INS-dominated estimate.
Consider scan matching in a long hallway, tunnel, or pipe. If the scene looks nearly identical as the sensor moves along one direction, then the lidar residuals may not distinguish motion well in that direction. For example, several poses along the corridor axis can explain the scan almost equally well.
After linearization, the optimization is usually written as
\[H \Delta x = b, \quad H = J^T W J.\]Here, $J$ is the Jacobian of the residuals and $W$ is the weighting matrix. Since $H = J^T W J$, the normal matrix is positive semidefinite when $W$ is positive semidefinite. If $H$ is rank deficient or badly ill-conditioned, then there exists a nonzero vector $v$ such that
\[H v \approx 0.\]That means the cost changes very little along the direction $v$, so the update is not uniquely determined by the measurements. This is the core of gauge freedom in local estimation.
Take a simplified 2D corridor. Let the state update be
\[\Delta x = \begin{bmatrix} \Delta s \\ \Delta n \end{bmatrix},\]where $s$ is motion along the corridor and $n$ is motion across the corridor. If the corridor is long and visually repetitive, lidar often constrains the cross-corridor direction much better than the along-corridor direction. A toy lidar-only normal matrix might look like
\[H_{\text{lidar}} = \begin{bmatrix} 0.01 & 0 \\ 0 & 12 \end{bmatrix}.\]This means:
- The cost changes only a little when we move along $s$.
- The cost changes a lot when we move along $n$.
- So the solution is numerically weak along the corridor axis.
Then the matrix is nearly singular, and updates in the $s$ direction are poorly determined.
Now suppose we add IMU constraints in a tightly coupled estimator. Over a short time interval, the IMU provides a motion prior and temporal consistency, so a simple toy contribution could look like
\[H_{\text{imu}} = \begin{bmatrix} 4 & 0 \\ 0 & 0.5 \end{bmatrix}.\]The fused normal matrix becomes
\[H_{\text{total}} = H_{\text{lidar}} + H_{\text{imu}} = \begin{bmatrix} 4.01 & 0 \\ 0 & 12.5 \end{bmatrix}.\]This is the key intuition:
- Lidar alone gave almost no information along $s$.
- IMU adds a short-horizon motion constraint, so the along-corridor direction is no longer almost free.
- The fused problem is better conditioned, even though global drift is still possible over long time scales.
This example is deliberately simplified. In a real LIO system, the state includes position, orientation, velocity, and IMU biases, so $H$ is much larger than $2 \times 2$. But the conditioning story is the same: the lidar block may be weak in some directions, and the IMU terms help stabilize the update.
