Lesson 4 · Inertial state estimation · 30–45 minutes first study · 15 minutes revision

The bias correction

Why the deltas go stale when the bias moves, and the first-order fix.

Lesson 3 left one thing unexplained: the _hat in glass-core's residual. The deltas in eq. (33) still contain the biases, and the solver keeps re-estimating the biases, so the deltas go stale. This lesson is Forster's fix, eq. (44): correct them to first order instead of re-integrating. It's also what keeps glass-lio's lesson-1 fix, estimating \(\mathbf{b}^{a}\) as a state, affordable.

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 the correction costs a few matrix products, not hundreds of samples.

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

The easiest one, and a good whiteboard moment. Start 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 100 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."

Say it at a whiteboard

"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 · 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?

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.