glass-lio courseLesson 3 › Reference card

Reference card · Lesson 3

IMU preintegration

From eq. (31) iterated, to three deltas reusable when the starting pose changes.

Key equations

Warm-up, from memory

$$ \mathbf{v}(t+\Delta t) = \mathbf{v}(t) + \mathbf{g}\,\Delta t + \mathbf{R}(t)\,(\tilde{\mathbf{a}} - \mathbf{b}^{a})\,\Delta t $$

Iterate one step, many times

$$ \begin{aligned} \mathbf{R}_j &= \mathbf{R}_i \prod_{k=i}^{j-1} \mathrm{Exp}\big((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}^{g})\,\Delta t\big)\\ \mathbf{v}_j &= \mathbf{v}_i + \mathbf{g}\,\Delta t_{ij} + \sum_{k=i}^{j-1} \mathbf{R}_k\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t \end{aligned} \tag{32} $$

The move

$$ \begin{aligned} \mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij} &= \mathbf{R}_i \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t\\ \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij}) &= \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t \end{aligned} $$

The three deltas

$$ \begin{aligned} \Delta\mathbf{R}_{ij} &\doteq \mathbf{R}_i^{\top}\mathbf{R}_j = \prod_{k=i}^{j-1} \mathrm{Exp}\big((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}^{g})\,\Delta t\big)\\ \Delta\mathbf{v}_{ij} &\doteq \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij})\\ &= \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t\\ \Delta\mathbf{p}_{ij} &\doteq \mathbf{R}_i^{\top}\big(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i\,\Delta t_{ij} - \tfrac{1}{2}\mathbf{g}\,\Delta t_{ij}^{2}\big)\\ &= \sum_{k=i}^{j-1} \Big[\Delta\mathbf{v}_{ik}\,\Delta t + \tfrac{1}{2}\,\Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t^{2}\Big] \end{aligned} \tag{33} $$

Practice

$$ \Delta\mathbf{v}_{ij} = \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij}) = \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t $$
$$ \Delta\mathbf{R}_{ij} = \mathbf{R}_i^{\top}\mathbf{R}_j = \prod_{k=i}^{j-1} \mathrm{Exp}\big((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}^{g})\,\Delta t\big) $$

Say it at a whiteboard

"Iterating the IMU from \(i\) to \(j\) puts \(\mathbf{R}_i\) inside every term, so each time the optimizer moves \(\mathbf{R}_i\) you'd re-integrate hundreds of samples. Preintegration writes \(\mathbf{R}_k = \mathbf{R}_i\,\Delta\mathbf{R}_{ik}\), pulls \(\mathbf{R}_i\) out of the sums, and moves the states and gravity to the other side. What's left, \(\Delta\mathbf{R}_{ij}\), \(\Delta\mathbf{v}_{ij}\) and \(\Delta\mathbf{p}_{ij}\), depends only on the IMU measurements and the bias. So it's integrated once, and the residual just compares it with the states."