The bias correction

Preintegrated deltas depend on the biases assumed when they were built. If an optimizer changes those biases, the deltas need updating. This lesson derives the first-order correction that avoids integrating every IMU sample again, including the rotation update on SO(3).

0 · Warm-up, from memory

The measurement side of $\Delta\mathbf{v}_{ij}$ from lesson 3, the sum this lesson differentiates. After a night's sleep, this box is the real test.

From memory: write the measurement side of $\Delta\mathbf{v}_{ij}$, the sum over samples.

1 · The deltas go stale

Every sum in eq. (33) contains $\mathbf{b}^{g}$ or $\mathbf{b}^{a}$. They're integrated once, at whatever bias estimate $\bar{\mathbf{b}}$ the solver held at the time. But the bias is a state, and the solver moves it. Forster: "more likely, the bias estimate changes by a small amount $\delta\mathbf{b}$ during optimization. One solution would be to recompute the delta measurements when the bias changes; however, that is computationally expensive."Forster et al., On-Manifold Preintegration…, §VI-C, "Incorporating Bias Updates".

glass-lio hits exactly this. Each scan's preintegration is built at the current bias estimate,lio_estimator.cpp, buildPreintegration(). The bias is a state because of lesson 1's fix: "let b_a be ESTIMATED" (glasslio_node.cpp). and then every Gauss-Newton iteration of the tight solve nudges that bias.

2 · Correct to first order

Rather than re-integrate, expand each delta to first order in the bias offset $\delta\mathbf{b} = \mathbf{b} - \bar{\mathbf{b}}$. This is Forster's eq. (44), with a tilde for the preintegrated measurement, as in lesson 1:

$$ \begin{aligned} \Delta\tilde{\mathbf{R}}_{ij}(\mathbf{b}) &\simeq \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}})\,\mathrm{Exp}\big(\mathbf{J}^{R}_{g}\,\delta\mathbf{b}^{g}\big)\\ \Delta\tilde{\mathbf{v}}_{ij}(\mathbf{b}) &\simeq \Delta\tilde{\mathbf{v}}_{ij}(\bar{\mathbf{b}}) + \mathbf{J}^{v}_{g}\,\delta\mathbf{b}^{g} + \mathbf{J}^{v}_{a}\,\delta\mathbf{b}^{a}\\ \Delta\tilde{\mathbf{p}}_{ij}(\mathbf{b}) &\simeq \Delta\tilde{\mathbf{p}}_{ij}(\bar{\mathbf{b}}) + \mathbf{J}^{p}_{g}\,\delta\mathbf{b}^{g} + \mathbf{J}^{p}_{a}\,\delta\mathbf{b}^{a} \end{aligned} \tag{44} $$

Each $\mathbf{J}$ is a Jacobian, evaluated at $\bar{\mathbf{b}}$: for example $\mathbf{J}^{v}_{a} \doteq \partial\Delta\bar{\mathbf{v}}_{ij}/\partial\mathbf{b}^{a}$.Forster writes the partial derivatives out in full; the $\mathbf{J}$ shorthand is this course's, for writing by hand. In glass-core, $\mathbf{J}^{v}_{a}$ is dv_dba_, $\mathbf{J}^{R}_{g}$ is dR_dbg_, and so on. Forster: "The Jacobians remain constant and can be precomputed during the preintegration." So each correction costs a few matrix products instead of re-integrating every sample in the interval.

3 · Rotations don't add

The velocity and position deltas live in $\mathbb{R}^3$, so their correction is plain addition. The rotation delta doesn't: a rotation matrix plus a small matrix isn't a rotation. So its correction is a small rotation vector, $\mathbf{J}^{R}_{g}\,\delta\mathbf{b}^{g}$, turned into a rotation by $\mathrm{Exp}$ and applied on the right. That's the same pattern as lesson 2's rotation update. Note too that $\Delta\tilde{\mathbf{R}}_{ij}$ depends only on the gyro bias, because it's built from the gyro alone.

glass-core's three corrections, one line each:preintegration.cpp, lines 96–119. The comments: "The correction lives in the tangent space at dR_, so it composes on the RIGHT", and, for velocity, "dv lives in R^3: plain addition".

return dR_ * SO3d::exp(dR_dbg_ * dbg);
return dv_ + dv_dbg_ * dbg + dv_dba_ * dba;
return dp_ + dp_dbg_ * dbg + dp_dba_ * dba;

4 · Derive one Jacobian yourself

Start with the easiest Jacobian, from the warm-up's sum. The accelerometer bias appears once, linearly, and $\Delta\bar{\mathbf{R}}_{ik}$ doesn't depend on it at all (gyro only):

$$ \begin{aligned} \Delta\bar{\mathbf{v}}_{ij} &= \sum_{k=i}^{j-1} \Delta\bar{\mathbf{R}}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t\\ \Longrightarrow\quad \mathbf{J}^{v}_{a} = \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^{a}} &= -\sum_{k=i}^{j-1} \Delta\bar{\mathbf{R}}_{ik}\,\Delta t \end{aligned} $$

That's the expression in Forster's Appendix IX-B, and glass-core builds it as a running sum, one sample at a time:preintegration.cpp, line 79. The gyro-bias Jacobians are harder, because $\mathbf{b}^{g}$ sits inside every $\Delta\mathbf{R}_{ik}$. They're accumulated in the same loop (lines 71–82), with their own update-order trap. The full list is on the reference card.

dv_dba_ += -dR_mat * dt;

5 · Applied as an offset, in the residual

Here's the _hat from lesson 3. The residual measures the offset from the bias the deltas were integrated with, then corrects:nav_residual.hpp, lines 50–56. glass-core's comment on the correction: "dbg/dba are the OFFSET from the bias these deltas were integrated with -- not the new bias itself."

const Eigen::Vector3d dbg = xj.bg - pre.bias_gyro();
const Eigen::Vector3d dba = xj.ba - pre.bias_accel();
const Sophus::SO3d dR_hat = pre.dR_corrected(dbg);
const Eigen::Vector3d dv_hat = pre.dv_corrected(dbg, dba);
const Eigen::Vector3d dp_hat = pre.dp_corrected(dbg, dba);

How good is first order? Forster tested it by integrating random IMU samples, perturbing the bias, and comparing the correction against a full re-integration: "The errors resulting from the first-order approximation are negligible, even for relatively large bias perturbations."Forster §VIII, the discussion of Fig. 11. If the bias ever drifts too far, glass-core's own rule is blunt: "the answer is to re-integrate, not to add higher-order terms."

Explain the idea

"The preintegrated deltas depend on the bias they were integrated with, and the solver keeps moving the bias. Rather than re-integrating, I also accumulate each delta's Jacobian with respect to the biases during preintegration. Then, for a bias offset $\delta\mathbf{b}$, $\Delta\mathbf{v}$ and $\Delta\mathbf{p}$ get a linear correction, and $\Delta\mathbf{R}$ is right-multiplied by the $\mathrm{Exp}$ of its Jacobian times $\delta\mathbf{b}^{g}$, because rotations don't add. The Jacobians are fixed at the integration bias, so it's cheap; if the bias drifts far, you re-integrate."

6 · The hard one: how the gyro bias moves $\Delta\mathbf{R}_{ij}$

Section 4 was easy because $\mathbf{b}^{a}$ enters linearly and outside every rotation. The gyro bias is the opposite: it sits inside every factor of a product of exponentials,

$$ \Delta\mathbf{R}_{ij}(\mathbf{b}^g) = \prod_{k=i}^{j-1}\mathrm{Exp}\big((\tilde{\boldsymbol{\omega}}_k - \bar{\mathbf{b}}^g - \delta\mathbf{b}^g)\,\Delta t\big). $$

Two tools from the toolkit take it apart. First, the right Jacobian splits each factor into the nominal step and a small correction: with $\boldsymbol{\phi}_k = (\tilde{\boldsymbol{\omega}}_k - \bar{\mathbf{b}}^g)\,\Delta t$ and $\mathbf{J}_r^k = \mathbf{J}_r(\boldsymbol{\phi}_k)$,

$$ \mathrm{Exp}(\boldsymbol{\phi}_k - \delta\mathbf{b}^g\Delta t) \approx \mathrm{Exp}(\boldsymbol{\phi}_k)\,\mathrm{Exp}\big(-\mathbf{J}_r^k\,\delta\mathbf{b}^g\,\Delta t\big). $$

Now the product is a chain of big rotations with small ones wedged between them. Second, the adjoint rule $\mathrm{Exp}(\mathbf{x})\,\mathbf{R} = \mathbf{R}\,\mathrm{Exp}(\mathbf{R}^\top\mathbf{x})$ lets you slide every small rotation to the far right of the product. Sliding the correction born at step $k$ past all the later steps rotates it by their combined rotation $\Delta\bar{\mathbf{R}}_{k+1,j}$. Collect them, keep first order, and

$$ \Delta\mathbf{R}_{ij}(\mathbf{b}^g) \approx \Delta\bar{\mathbf{R}}_{ij}\,\mathrm{Exp}\big(\mathbf{J}^R_g\,\delta\mathbf{b}^g\big), \qquad \mathbf{J}^R_g = -\sum_{k=i}^{j-1}\Delta\bar{\mathbf{R}}_{k+1,j}^\top\,\mathbf{J}_r^k\,\Delta t. $$

Each sample's bias error is weighted by how much rotation happened after it, which is why the formula cannot be a simple sum of $-\Delta t$ terms. Written as a running update, adding one sample at a time, it becomes a recursion that needs no stored history:

$$ \mathbf{J}^R_{g,\,i,k+1} = \mathrm{Exp}(\boldsymbol{\phi}_k)^\top\,\mathbf{J}^R_{g,\,i,k} - \mathbf{J}_r^k\,\Delta t, $$

which is exactly glass-core's line dR_dbg_ = dRk.matrix().transpose() * dR_dbg_ - Jr * dt;.preintegration.cpp, line 82. The block comment above it (lines 71–75) states the order trap. The velocity and position gyro Jacobians use the old $\mathbf{J}^R_g$ at each step, because the bias reaches velocity only through the rotation that was current when that sample's specific force was applied. So the rotation Jacobian must be advanced last. Advance it first and every velocity and position Jacobian silently uses the rotation error of the wrong step: no crash, and a bias correction that is subtly wrong.

Try it: what is $\mathbf{J}^R_g$ when the sensor does not rotate at all?

With $\boldsymbol{\phi}_k = 0$, every $\mathbf{J}_r^k = \mathbf{I}$ and every $\Delta\bar{\mathbf{R}} = \mathbf{I}$, so $\mathbf{J}^R_g = -\Delta t_{ij}\,\mathbf{I}$. A gyro bias error $\delta\mathbf{b}^g$ held for $\Delta t_{ij}$ seconds produces a rotation error of $-\delta\mathbf{b}^g\,\Delta t_{ij}$: rate times time, as it should be.

7 · How good is first order?

Forster's claim that the first-order errors are “negligible” can be checked directly: integrate a realistic motion, perturb both biases, and compare eq. (44) against a full re-integration at the new bias. The gap between the two is the approximation error. Three things come out of such a check:

The error is quadratic in the offset. Scale the offset up and the error grows with its square. That is the signature of a correct first-order expansion, and it is also the best test for your own Jacobians: if the leftover error shrinks only linearly as the offset shrinks, a Jacobian is wrong, because a wrong Jacobian leaves a first-order error behind.
For glass-lio it is negligible. One scan is a short interval, and the bias random walk lets the bias move only a little between scans, so the offsets the correction ever sees are tiny, and so are the errors.
Long intervals and large corrections are where it breaks. Over a long interval with a large offset, first order misses a noticeable fraction of the velocity change. A sliding window whose early bias estimate is poor can land there, and that is exactly the case glass-core's rule is for: re-integrate rather than add higher-order terms.

8 · Why expand around $\bar{\mathbf{b}}$, and when $\bar{\mathbf{b}}$ is reset

A natural question: why expand around the bias the deltas were integrated with, rather than around the solver's current estimate? Because the Jacobians are only known at $\bar{\mathbf{b}}$. They were accumulated during the same pass that built the deltas, with every $\mathbf{J}_r^k$ and every $\Delta\bar{\mathbf{R}}_{ik}$ evaluated at $\bar{\mathbf{b}}$. Expanding around any other point would need Jacobians at that point, which means another pass over the samples: exactly the re-integration eq. (44) exists to avoid. So $\bar{\mathbf{b}}$ is fixed for the life of one preintegration, and $\delta\mathbf{b}$ is always measured from it.

How large can $\delta\mathbf{b}$ grow? That depends on how long a preintegration lives, and here glass-lio makes a choice that keeps it tiny. Each scan's preintegration is built fresh, at the bias estimate the state holds at that scan:lio_estimator.cpp, line 253: ImuPreintegration pre(state_.bg, state_.ba, ...).

ImuPreintegration pre(state_.bg, state_.ba, p_.gyro_noise, p_.accel_noise);

So $\bar{\mathbf{b}}$ is effectively reset every scan, and $\delta\mathbf{b}$ only accumulates across the Gauss-Newton iterations of a single scan's solve. In steady state that is bounded by what the bias random walk and one scan of evidence allow: tiny, deep in the regime where section 7 shows the error is negligible. The correction is still necessary, because the solver does move the bias within a scan and the deltas must follow it consistently, but it is never at risk of being inaccurate.

A sliding window has no such luxury. A factor built once lives through many solves while the bias estimate keeps refining, sometimes by a lot early on. Windowed estimators therefore watch $|\delta\mathbf{b}|$ and re-integrate the factor, resetting $\bar{\mathbf{b}}$, when it grows past a threshold. Both designs follow the same rule; they differ only in how often the rule has to fire.

Try it: why is a stale $\bar{\mathbf{b}}$ harmless to the residual's value but not to its accuracy?

The residual always uses $\bar{\mathbf{b}} + \delta\mathbf{b}$, the solver's current bias, so it evaluates at the right bias whatever $\bar{\mathbf{b}}$ is. What degrades with a large offset is the first-order approximation of the deltas at that bias. The answer is correct in form and increasingly wrong in detail, which is exactly why the drift is silent.

9 · What the bias Jacobians say about observability

The Jacobians are not just a computational shortcut; they are a statement of how visible each bias is. For short intervals with little rotation, both reduce to “rate times time”:

$$ \mathbf{J}^R_g \approx -\Delta t_{ij}\,\mathbf{I},\qquad \mathbf{J}^v_a \approx -\Delta t_{ij}\,\mathbf{I}. $$

The gyro bias is easy to see. A gyro-bias error rotates the prediction by $\delta\mathbf{b}^g\,\Delta t_{ij}$, and the LiDAR measures rotation directly, every scan. So the solver can check the gyro bias against geometry at every scan, which is why glass-lio's gyro-bias estimate is trusted enough to be fed back into deskewing when the scene constrains rotation.

The accelerometer bias is hard to see. An accelerometer-bias error shifts the predicted velocity by $-\mathbf{R}_i\,\delta\mathbf{b}^a\,\Delta t_{ij}$. But a small tilt of gravity by an angle $\theta$ shifts it by $g\,\theta\,\Delta t_{ij}$ sideways, which has exactly the same shape. A bias $\delta\mathbf{b}^a$ is indistinguishable, over one interval, from a gravity tilt of $|\delta\mathbf{b}^a|/g$ radians. The only thing that separates them is lesson 1's observation: the bias turns with the body and gravity does not. So the accelerometer bias becomes observable only across intervals with different orientations, and only as fast as the platform turns. On a robot driving straight down a corridor, it barely is.

Why this matters in glass-lio

This is why glass-lio starts the accelerometer bias uncertain and lets the data reveal it slowly, and why a random-walk prior stops it from absorbing whatever else is wrong. A free, weakly observable bias is a magnet for errors that belong elsewhere; the project's tight-coupling post-mortem records a bias that did exactly that when it was loosened too far.

10 · Practice

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

Why doesn't glass-core simply re-integrate the deltas whenever the bias estimate changes?

Why is $\Delta\tilde{\mathbf{R}}_{ij}$ corrected as $\Delta\tilde{\mathbf{R}}_{ij}\,\mathrm{Exp}(\mathbf{J}^{R}_{g}\,\delta\mathbf{b}^{g})$ and not as $\Delta\tilde{\mathbf{R}}_{ij} + \mathbf{J}^{R}_{g}\,\delta\mathbf{b}^{g}$?

Differentiate $\Delta\bar{\mathbf{v}}_{ij} = \sum_k \Delta\bar{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t$ with respect to $\mathbf{b}^{a}$. What is $\mathbf{J}^{v}_{a}$?

In glass-core's residual, what is the dbg passed to dR_corrected()?

From lesson 3. Which preintegrated delta corresponds to a true physical quantity?

You halve the bias offset and the gap between eq. (44) and a full re-integration halves too. What does that tell you?

In glass-core's accumulation loop, when must the rotation's gyro-bias Jacobian dR_dbg_ be advanced?

What makes the accelerometer bias separable from a small tilt of gravity?

From memory: write the three lines of eq. (44) using the $\mathbf{J}$ shorthand. Then say which line is not a plain addition, and why.