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