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