glass-lio courseLesson 4 › Reference card

Reference card · Lesson 4

Bias correction

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

Key equations

Warm-up, from memory

$$ \Delta\mathbf{v}_{ij} = \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t $$

Correct to first order

$$ \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} $$

Derive one Jacobian yourself

$$ \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} $$

Practice

$$ \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} $$

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."