Integrating one IMU step

Lesson 1 ended on a question. glass-core's integrate() subtracts the biases but never touches gravity, so where does $\mathbf{g}$ come back? This lesson answers it by building Forster's eq. (31): the update that turns one IMU sample into a change of orientation, velocity and position. If an interviewer says "integrate this IMU sample", this is what you write first.

0 · Warm-up, from memory

Before anything new, one retrieval from last time. Don't open lesson 1.

From memory: write eq. (28), the accelerometer's measurement model.

1 · How the state moves

Forster writes the kinematics of orientation, velocity and position like this:Forster et al., On-Manifold Preintegration…, §V, eq. (29). The hat $(\cdot)^{\wedge}$ turns a 3-vector into its skew-symmetric matrix (§III-A).

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

Velocity and position live in the world frame and are plain calculus. The rotation equation is the interesting one: the angular rate is measured in the body frame, so it multiplies $\mathbf{R}_{WB}$ on the right.

2 · Hold everything constant for one step

An IMU samples every $\Delta t$. Assume $\boldsymbol{\omega}$ and $\mathbf{a}$ stay constant across one sample interval, and eq. (29) integrates in closed form:Forster §V, eq. (30).

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

The last two lines are the constant-acceleration formulas from school physics. The first uses $\mathrm{Exp}(\boldsymbol{\phi}) = \exp(\boldsymbol{\phi}^{\wedge})$, which turns a rotation vector (axis times angle) into a rotation matrix.Forster eq. (6). For the Lie-theory side, see Solà, Deray & Atchuthan, A micro Lie theory for state estimation in robotics. Here $\boldsymbol{\omega}\,\Delta t$ is the small rotation the body turned through in one step. Multiplying on the right applies it in the body frame, where the gyroscope measured it.

3 · Put the IMU in: gravity comes back

Eq. (30) needs the true $\boldsymbol{\omega}$ and $\mathbf{a}$, but the IMU only gives measurements. So invert eqs. (27) and (28). The gyroscope is one subtraction. The accelerometer takes three short steps:

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

The middle step multiplies both sides by $\mathbf{R}$, using $\mathbf{R}\mathbf{R}^{\top} = \mathbf{I}$. That answers lesson 1's question: gravity comes back here, outside $\mathbf{R}$. $\mathbf{R}$ carries the bias-corrected specific force out of the body frame into the world, and gravity was never in the body frame, so there's nothing to rotate.

Substitute both into eq. (30) and you get Forster's discrete-time IMU update:Forster §V, eq. (31). The superscript $d$ marks discrete-time noise: over one step, $\mathrm{Cov}(\boldsymbol{\eta}^{gd}) = \mathrm{Cov}(\boldsymbol{\eta}^{g})/\Delta t$. glass-core builds exactly that, $\sigma^2/\Delta t$, in preintegration.cpp, lines 59–66.

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

The measurements, biases and noise are all taken at time $t$: Forster writes the $(t)$ out, and it's dropped here to fit the page. Read it term by term: gravity appears as $\mathbf{g}\,\Delta t$ and $\tfrac{1}{2}\mathbf{g}\,\Delta t^{2}$, unrotated, because it's a constant world-frame acceleration. The IMU's contribution is rotated by $\mathbf{R}(t)$.Holding $\mathbf{R}(t)$ fixed across the step is not exact while the IMU rotates. Forster notes that a high-rate IMU keeps the error small, and that slower IMUs may call for higher-order integration (§V).

4 · The update-order trap

Look at what sits on the right-hand side of eq. (31): everything is at time $t$, the start of the step. Velocity and position both use $\mathbf{R}(t)$, not the rotation you've just computed, and position uses $\mathbf{v}(t)$, not the new velocity. In code, that forces the order: position first, then velocity, rotation last.

glass-core does exactly this.preintegration.cpp, lines 85–90. Its comment: "Update dR first and you have integrated position with the wrong orientation." And on the last line: "compose ON the manifold, on the RIGHT (body-frame increment)". Here $\mathbf{R}(t)$ becomes dR_, the orientation relative to the start of the preintegration window, and $\mathbf{g}$ is left out:

dp_ += dv_ * dt + 0.5 * dR_mat * a * dt * dt;
dv_ += dR_mat * a * dt;
dR_ = dR_ * dRk;

5 · Where glass-core puts gravity back

The deltas are gravity-free, so gravity has to return somewhere. It comes back once, over the whole interval, in predictState():nav_residual.hpp, lines 152–162.

xj.R = xi.R * pre.dR();
xj.v = xi.v + gravity * dt + xi.R * pre.dv();
xj.p = xi.p + xi.v * dt + 0.5 * gravity * dt * dt + xi.R * pre.dp();

It's the same shape as eq. (31): gravity unrotated in the world frame, and the IMU part rotated by the orientation at the start, xi.R. The code's comment says why gravity was kept out of the deltas: "so the deltas would not depend on xi". Why that matters is lesson 3.

Say it at a whiteboard

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

6 · Practice

From memory, no scrolling. The last question comes from lesson 1: mixing old and new material is what makes it stick.

Solve eq. (28) for the true world acceleration $\mathbf{a}$, ignoring noise. Which is right?

In the velocity line of eq. (31), why isn't $\mathbf{g}$ multiplied by $\mathbf{R}$?

In eq. (31), and in glass-core's integrate(), which orientation multiplies the specific force in the velocity and position updates?

Why does the rotation update right-multiply, $\mathbf{R}(t)\,\mathrm{Exp}(\boldsymbol{\omega}\Delta t)$, rather than $\mathrm{Exp}(\boldsymbol{\omega}\Delta t)\,\mathbf{R}(t)$?

From lesson 1. A level IMU accelerates straight up at 1 g. What does its $z$ axis read?

From memory: write the three lines of eq. (31), bias-corrected, with the noise dropped. Then say the whiteboard sentence out loud.

Apply it: complete the practical worksheet exercise before moving on. Derive an answer and test it on the supplied inputs.