The noise preintegration carries

Preintegrated deltas describe the IMU prediction; their 9×9 covariance describes its uncertainty. Propagate that covariance one sample at a time, then use it to weight the IMU residual against other measurements.

0 · Warm-up, from memory

The position line of eq. (31), from lesson 2. The noise update in this lesson has exactly its shape.

1 · Why a covariance at all

Least squares weights each residual by its inverse covariance. Forster calls modelling it "of paramount importance… the inverse noise covariance is used to weight the terms in the optimization."Forster et al., On-Manifold Preintegration…, §VI-B. glass-lio does exactly that. The IMU factor's weight is the inverse of the preintegration covariance, while each LiDAR point is whitened by lidar_sigma:tight_registration.cpp, lines 118–129. glass-lio also adds the previous state's own uncertainty, \(\mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^{\top}\), which marginalizes an independent Gaussian perturbation of x_i from this one linearized factor. The LiDAR side is at lines 64–70.

const Eigen::Matrix<double, 9, 9> sigma_eff =
  pre.covariance() + Ji * xi_cov * Ji.transpose();
Eigen::Matrix<double, 9, 9> imu_information =
  sigma_eff.inverse() * params.imu_prior_weight;

This matrix helps set the balance with LiDAR. The covariance inflation is exact for that local linear Gaussian model with independent noise; it is not the marginalization of every historical factor or a proof of consistency of the whole estimator.

2 · The noise is linear, so propagate it linearly

Forster writes each delta as "the (to-be-estimated) state 'plus' a random noise", stacked as \(\boldsymbol{\eta}^{\Delta} = [\delta\boldsymbol{\phi};\ \delta\mathbf{v};\ \delta\mathbf{p}]\), and shows that, to first order, this is "a linear function of the IMU measurement noise". That makes the covariance "a simple linear propagation". Better still, it can be done iteratively: "as a new IMU measurement arrive[s] we only update \(\Sigma_{ij}\), rather than recomputing it from scratch."Forster §VI-A (eq. 38) and §VI-B (eqs. 39–43). The iterative form is Appendix IX-A.

3 · One sample at a time

With \(\mathbf{a}_k \doteq \tilde{\mathbf{a}}_k - \mathbf{b}^{a}\) and the raw IMU noise \(\boldsymbol{\eta}^{d}_k = [\boldsymbol{\eta}^{gd}_k;\ \boldsymbol{\eta}^{ad}_k]\), each sample updates the noise, and so the covariance, like this:Forster Appendix IX-A, eqs. (59)–(63), written there from \(j-1\) to \(j\); here from \(k\) to \(k+1\). \(\Delta\tilde{\mathbf{R}}_{k,k+1}\) is this step's rotation, and \(\Delta\tilde{\mathbf{R}}_{ik}\) the rotation accumulated before it.

It starts at zero because an empty interval has measured nothing yet. The raw noise covariance \(\Sigma_{\eta}\) comes from the datasheet densities, turned into discrete-time noise exactly as in lesson 2: \(\sigma^{2}/\Delta t\).

Here \(\mathbf{J}_r^k=\mathbf{J}_r((\tilde{\boldsymbol{\omega}}_k-\mathbf{b}^g)\Delta t)\) is the right Jacobian of SO(3): it converts an additive error in a rotation vector into a small right-multiplied rotation. It tends to \(\mathbf{I}\) near zero.

The covariance recursion assumes the new measurement noise is independent of the accumulated error. The densities use the convention \(Q_d=\operatorname{diag}(\sigma_g^2\mathbf I,\sigma_a^2\mathbf I)/\Delta t\). This 9×9 matrix conditions on the chosen integration biases; it does not include their uncertainty or random walk.

4 · Read it like eq. (31)

Don't memorise A and B as matrices. Read them row by row, and they're the warm-up's equation, linearised:

• Position row: the error picks up the old velocity error times \(\Delta t\) (the \(\mathbf{I}\,\Delta t\)), and accelerometer noise enters with \(\tfrac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\,\Delta t^{2}\), exactly where the specific force does in (31).
• Velocity row: accelerometer noise enters with \(\Delta\tilde{\mathbf{R}}_{ik}\,\Delta t\), again mirroring (31).
• The first column: an orientation error tilts the specific force slightly the wrong way. To first order that's \(-\Delta\tilde{\mathbf{R}}_{ik}\,\mathbf{a}_k^{\wedge}\,\delta\boldsymbol{\phi}\), fed into velocity and position.
• Rotation row: the old error, re-expressed in the new frame, plus fresh gyro noise.

glass-core builds exactly these blocks, term for term:preintegration.cpp, lines 40–68. It runs before the state update in the same loop: "it linearises about the CURRENT state, so it needs dR before we advance it." That's the update-order trap from lesson 2, again.

A.block<3, 3>(0, 0) = dRk.matrix().transpose();
A.block<3, 3>(3, 0) = -dR_mat * a_hat * dt;
A.block<3, 3>(6, 0) = -0.5 * dR_mat * a_hat * dt * dt;
A.block<3, 3>(6, 3) = Eigen::Matrix3d::Identity() * dt;

B.block<3, 3>(0, 0) = Jr * dt;
B.block<3, 3>(3, 3) = dR_mat * dt;
B.block<3, 3>(6, 3) = 0.5 * dR_mat * dt * dt;

cov_ = A * cov_ * A.transpose() + B * Q * B.transpose();

5 · Accumulating noise is not a monotonicity theorem

Each sample adds a positive semidefinite term \(\mathbf B\Sigma_\eta\mathbf B^\top\) to the propagated covariance \(\mathbf A\Sigma\mathbf A^\top\). This does not imply \(\Sigma_{k+1}-\Sigma_k\succeq0\): propagation rotates and couples errors, so individual directional variances need not increase at every step.

A rotation, for instance, can move variance from one axis onto another, so the variance along one axis can fall even though fresh noise was added. Longer inertial dead reckoning generally accumulates error, but that practical trend is not an ordering of every covariance matrix or its inverse.

In the simpler scalar model \(v_{k+1}=v_k+\eta_k\Delta t\), with independent \(\operatorname{Var}(\eta_k)=\sigma_a^2/\Delta t\), the variance is exactly \(\sigma_a^2T\). Doubling \(T\) doubles that variance and halves its information. State the model before drawing this conclusion.

Noise densities are physical parameters to measure or obtain from a suitable sensor specification. Validate their units, bandwidth assumptions, and the resulting residual statistics. Datasheet range noise alone is not the full point-to-plane residual uncertainty: plane fitting, deskew errors, and correlated returns also matter. The historical glass-lio settings are calibration choices for those experiments, not universal constants.

6 · What one scan's covariance looks like

Run the recursion of section 3 over one glass-lio scan and read the standard deviations off the diagonal of \(\Sigma_{ij}\). Over one scan all three blocks are small: the gyro alone knows the scan's rotation extremely well, and the velocity and position uncertainties are tiny compared with what a LiDAR can resolve. For a non-rotating sensor they follow simple formulas you could derive by hand: \(\sigma_g\sqrt{T}\) for rotation, \(\sigma_a\sqrt{T}\) for velocity, and \(\sigma_a T^{3/2}/\sqrt3\) for position.

The one coupling the recursion adds, orientation error tilting the specific force, is present but weak over a single scan. Run the same recursion for much longer and that coupling becomes the dominant term, which is lesson 2's \(t^3\) drift seen from the covariance side.

A test the recursion must pass: the sampling rate must not matter

Run the same interval at twice the IMU rate. Each step's discrete noise \(\sigma^2/\Delta t\) doubles, and there are twice as many steps, each weighted by a smaller \(\Delta t\) or \(\Delta t^2\) in \(\mathbf{B}_k\). The final covariance comes out essentially the same.

That is not a coincidence; it is what a noise density means. The physical uncertainty after a given time cannot depend on how often you happened to sample it. This makes an excellent unit test, and it catches the single most common mistake in writing this code: multiplying by \(\Delta t\) instead of dividing. With \(\sigma^2\Delta t\), a higher rate would shrink the variance, and a faster IMU would appear to make the estimator more certain for free.

7 · “Who wins”: the reasoning behind a real calibration fix

The covariance's inverse is the IMU's information. Each LiDAR point-to-plane correspondence, whitened by lidar_sigma, carries \(1/\sigma^2\) along its plane's normal. In the tight solve these two kinds of information are simply added, so their ratio decides which sensor wins a disagreement, direction by direction. Because information goes as \(1/\sigma^2\), shrinking lidar_sigma by some factor multiplies the LiDAR's say in every argument by the square of that factor.

This is not hypothetical. With lidar_sigma set well above the Livox's real point-to-plane noise, glass-lio's tight coupling let the IMU win arguments it should have lost. The point-to-plane residual has no velocity columns, so position is the only channel that disciplines velocity; with position under-trusted, the IMU over-integrated velocity to implausible speeds and the trajectory drifted far from the truth. Setting lidar_sigma to the sensor's actual noise brought it level with the loosely coupled baseline, with sane velocities.docs/implementation/testing.md, §12.5, “The last drift — an under-trusted LiDAR”.

8 · What this covariance does not contain

Section 3 already warned that \(\Sigma_{ij}\) is conditioned on the integration biases. It is worth listing everything it leaves out, because each omission is handled somewhere else, and forgetting where is how covariances become over-confident.

• Bias uncertainty. Handled by the bias states and their random-walk prior, not by \(\Sigma_{ij}\). The bias Jacobians of lesson 4 are how bias uncertainty reaches the residual.
• Uncertainty in the starting state \(\mathbf{x}_i\). Handled in glass-lio by the inflation \(\mathbf{J}_i\mathbf{P}_i\mathbf{J}_i^\top\) from section 1, or in a window by estimating \(\mathbf{x}_i\) jointly. Leaving it out is exactly the “infinitely certain \(\mathbf{x}_i\)” failure lesson 3 ended on.
• Non-white noise. Vibration from motors or wheels is correlated in time, not white. The recursion assumes independence between samples, so a vibrating platform needs larger effective densities than the datasheet lists, measured on the platform.
• Discretisation error. Lesson 2's “hold everything constant” error is systematic, not random, so no covariance models it. On a high-rate IMU it is small enough not to matter; on a slow IMU it would silently bias the estimate while the covariance claimed confidence.

9 · Practice

From memory, no scrolling. The last question comes from lesson 4.