Reference card · Lesson 2
IMU motion integration
Where gravity comes back in, and the update-order trap.
Key equations
Warm-up, from memory
$$ {}_{B}\tilde{\mathbf{a}} = \mathbf{R}_{WB}^{\top}\big({}_{W}\mathbf{a} - {}_{W}\mathbf{g}\big) + \mathbf{b}^{a} + \boldsymbol{\eta}^{a} $$
How the state moves
$$ \dot{\mathbf{R}}_{WB} = \mathbf{R}_{WB}\;{}_{B}\boldsymbol{\omega}_{WB}^{\wedge}, \qquad {}_{W}\dot{\mathbf{v}} = {}_{W}\mathbf{a}, \qquad {}_{W}\dot{\mathbf{p}} = {}_{W}\mathbf{v} \tag{29} $$
Hold everything constant for one step
$$ \begin{aligned}
\mathbf{R}(t+\Delta t) &= \mathbf{R}(t)\,\mathrm{Exp}\big(\boldsymbol{\omega}(t)\,\Delta t\big)\\
\mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{a}(t)\,\Delta t\\
\mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t)\,\Delta t + \tfrac{1}{2}\,\mathbf{a}(t)\,\Delta t^{2}
\end{aligned} \tag{30} $$
Put the IMU in: gravity comes back
$$ \begin{aligned}
\mathbf{R}^{\top}(\mathbf{a} - \mathbf{g}) &= \tilde{\mathbf{a}} - \mathbf{b}^{a} - \boldsymbol{\eta}^{a}\\
\Longrightarrow\quad \mathbf{a} - \mathbf{g} &= \mathbf{R}\,(\tilde{\mathbf{a}} - \mathbf{b}^{a} - \boldsymbol{\eta}^{a})\\
\Longrightarrow\quad \mathbf{a} &= \mathbf{R}\,(\tilde{\mathbf{a}} - \mathbf{b}^{a} - \boldsymbol{\eta}^{a}) + \mathbf{g}
\end{aligned} $$
$$ \begin{aligned}
\mathbf{R}(t+\Delta t) &= \mathbf{R}(t)\,\mathrm{Exp}\big((\tilde{\boldsymbol{\omega}} - \mathbf{b}^{g} - \boldsymbol{\eta}^{gd})\,\Delta t\big)\\
\mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{g}\,\Delta t + \mathbf{R}(t)\,(\tilde{\mathbf{a}} - \mathbf{b}^{a} - \boldsymbol{\eta}^{ad})\,\Delta t\\
\mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t)\,\Delta t + \tfrac{1}{2}\,\mathbf{g}\,\Delta t^{2}\\
&\quad + \tfrac{1}{2}\,\mathbf{R}(t)\,(\tilde{\mathbf{a}} - \mathbf{b}^{a} - \boldsymbol{\eta}^{ad})\,\Delta t^{2}
\end{aligned} \tag{31} $$
Practice
$$ \begin{aligned}
\mathbf{R}(t+\Delta t) &= \mathbf{R}(t)\,\mathrm{Exp}\big((\tilde{\boldsymbol{\omega}} - \mathbf{b}^{g})\,\Delta t\big)\\
\mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{g}\,\Delta t + \mathbf{R}(t)(\tilde{\mathbf{a}} - \mathbf{b}^{a})\,\Delta t\\
\mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t)\,\Delta t + \tfrac{1}{2}\mathbf{g}\,\Delta t^{2}\\
&\quad + \tfrac{1}{2}\mathbf{R}(t)(\tilde{\mathbf{a}} - \mathbf{b}^{a})\,\Delta t^{2}
\end{aligned} $$
Explain the idea
"For one IMU step, orientation is right-multiplied by the Exp of the bias-corrected gyro
times \(\Delta t\), because the rate is in the body frame. For velocity and position, I rotate
the bias-corrected specific force into the world with the orientation at the start of the step,
then add gravity back unrotated, because gravity lives in the world frame. Everything on the
right-hand side is at the start of the step, so in code I update position, then velocity, and
rotation last."