Integrating one IMU step

Lesson 1 separated measured specific force from world-frame acceleration. This lesson puts gravity back into a state update and derives the changes in orientation, velocity, and position over one IMU sample. The result is the basic propagation step used by inertial estimators.

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.

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

6 · The first test for any integrator

Symbols hide mistakes that a simple physical check exposes. Feed eq. (31) a level, stationary sensor with known biases. The accelerometer reads gravity's reaction, pointing up; rotated by $\mathbf{R} = \mathbf{I}$ and added to $\mathbf{g}$, it cancels exactly, and the velocity does not change. The robot does not fall through the floor.

This cancellation is the single best sanity check on an IMU integrator: feed it a stationary sensor and nothing may move. If your integrator drifts at rest, you have the sign of lesson 1 wrong, or the units, or you rotated gravity. It also shows where drift comes from in general: over one step, position is dominated by the velocity the platform already has, and velocity by gravity and specific force nearly cancelling. Small errors in that near-cancellation are what grow into drift, which is why the next sections care about details that look negligible.

The same check shows why the initial orientation matters. Tilt the sensor but leave $\mathbf{R}(t)$ at the identity, and part of gravity's reaction lands on a horizontal axis, where it is integrated as a forward acceleration the robot never had. Getting $\mathbf{R}(t)$ right is not optional; this is why initialisation levels the world first.

7 · How good is “hold everything constant”?

Eq. (30) assumes $\boldsymbol{\omega}$ and $\mathbf{a}$ are constant across the step, and eq. (31) rotates the specific force with the orientation at the start of the step. While the sensor turns, the true orientation drifts away from $\mathbf{R}(t)$ during the step, by up to $|\boldsymbol{\omega}|\,\Delta t$. So the specific force is applied in a direction that is slightly off, and for a specific force of about $g$ that acts like a small spurious acceleration of roughly $g\,|\boldsymbol{\omega}|\,\Delta t/2$.

The error grows with the turn rate and with the step length. For a high-rate IMU, such as the Mid-360's, holding everything constant is comfortably inside the noise and bias budget, so the simplest scheme is the right one. On a low-rate IMU mounted on something that spins, the same scheme would feed the bias estimator an error comparable to the bias it is trying to find, and a midpoint scheme (average the start and end orientations) would be worth its complexity. This is Forster's remark that a high-rate IMU keeps the error small and slower IMUs may need higher-order integration.

The two schemes also improve differently with rate. Eq. (31) is a first-order method: its error shrinks in proportion to the step. The midpoint scheme is second-order: its error shrinks with the square of the step. A second-order scheme at a low rate can beat a first-order one at a much higher rate. When the IMU rate is fixed by hardware, the integrator is the knob you actually have.

Two integrators in one codebase

glass-lio integrates the gyro in two places, and they deliberately differ. The deskew integrator, which only needs rotation within one scan, averages consecutive gyro samples (the trapezoidal rule, second-order accurate) and composes on the right.gyr_int.cpp, lines 54–55. The preintegration that feeds the tight solve holds each sample constant over its interval, a zero-order hold, exactly like eq. (31).lio_estimator.cpp, line 267: “zero-order hold over the interval”. The first is cheap to improve because it only touches rotation. The second has to stay matched to the Jacobians and the covariance built on eq. (31) in lessons 4 and 5, so changing its integration scheme would mean changing all of those too. Consistency between the integrator and its derivatives is worth more than a better integrator on its own.

8 · Which samples, over which interval?

Eq. (31) tells you how to apply one sample. The question that actually caused a bug in glass-lio is which samples to apply, and over exactly how long.

A LiDAR scan lasts one scan period. To deskew it, the synchroniser deliberately hands the estimator more IMU than that: one sample before the scan starts, so the gyro can be interpolated at the first point, and coverage past the end. That extra coverage is right for deskewing. It is wrong for preintegration. The tight solve relates the pose at the end of one scan to the pose at the end of the next, which are exactly one scan period apart. Integrating all of the IMU tells the solver “you moved this much” over an interval longer than the one the poses span.

The result is a claim of more motion than really happened, every scan, always in the direction of travel. The estimator accelerated away, and because the error scaled with speed and compounded, it looked exactly like a gravity problem. The fix was to clip every sample's interval to the window between the previous scan's end and this scan's end, including partial intervals at both edges:lio_estimator.cpp, lines 237–276: the comment block and the clipping loop.

const double lo = std::max(ta, t_begin);
const double hi = std::min(tb, t_end);
const double dt = hi - lo;
if (dt <= 0.0 || dt > kMaxImuGapSec) {
  continue;   // outside the window, duplicate, out of order, or a hole
}

Notice the second condition. A gap between two samples longer than a threshold is a hole in the stream, not a sample interval. Holding one reading constant across a hole would invent motion, so the loop skips it and lets the preintegration's covariance, lesson 5, reflect that less was measured.

The general lesson

Every integrated quantity has an interval, and the interval is part of the measurement. When two parts of a pipeline need different windows over the same stream (here, deskew and preintegration), give each its own window explicitly. Sharing one because it is convenient is how a systematic error hides in plain sight.

9 · Why the IMU cannot hold position alone

Eq. (31) is exact about the sensor you give it and blind to the errors you do not. Iterate it and each error source grows at its own characteristic rate:

a constant accelerometer bias is integrated twice: $\tfrac12 b_a t^2$;
accelerometer white noise is integrated twice too, but partly averages out: it grows as $t^{3/2}$;
a gyro bias is integrated once into a tilt, the tilt leaks gravity into the horizontal (lesson 1's tilted-sensor case), and that is integrated twice more: $t^3$.

The exponents are the story. The gyro term is the smallest over a short interval and by far the largest over a long one.

Over the short interval between two LiDAR scans, every one of these errors is far below what a LiDAR can resolve. Over a minute they are enormous. That asymmetry is the entire design of a LiDAR-inertial odometry: the IMU is excellent over short intervals and useless over long ones, so let it bridge the gap between two scans and let the LiDAR correct it at every scan. Lesson 3 turns “bridge the gap between two scans” into a single reusable measurement.

Try it: why does the bias term use $t^2/2$ while the gyro term uses $t^3/6$?

A constant accelerometer bias is an acceleration: integrate twice, $\iint b_a = b_a t^2/2$. A constant gyro bias gives a tilt that grows, $\theta = b_g t$, and so a horizontal acceleration $g\,b_g t$ that grows linearly; integrating that twice gives $g\,b_g t^3/6$. Each integration adds one power of $t$.

10 · 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 $g$. What does its $z$ axis read?

A level, stationary IMU with perfectly known biases is pushed through one step of eq. (31). What happens to the velocity?

The available IMU samples cover a longer interval than the scan-to-scan pose change. Why clip the preintegration to a shorter interval?

A low-rate IMU on a fast-spinning platform is integrated with eq. (31). The error from holding $\mathbf{R}(t)$ fixed across each step is…

From memory: write the three lines of eq. (31), bias-corrected, with the noise dropped. Then explain the principle in your own words.