IMU Preintegration

• IMU Preintegration
  • What problems does it solve
• Visual-inertial state estimation as MAP
  • The state
  • The measurements
  • Factor graph and MAP estimation
• IMU model and motion integration
  • What an IMU measures
  • Why the minus gravity
  • Kinematic model and integration
• IMU preintegration on manifold
  • Integrating from one keyframe to the next
  • Relative motion increments
  • Preintegrated IMU measurements
  • Noise propagation
  • Incorporating bias updates
  • Preintegrated IMU factors
  • Bias model
• Usage in GTSAM
• Reference

An IMU produces hundreds of measurements per second, a camera a few dozen frames, and an optimizer only wants to keep a state every few frames. IMU preintegration is the trick that bridges these rates: it summarizes all IMU measurements between two keyframes into one relative-motion measurement that does not need to be recomputed every time the optimizer moves the keyframe states.

This post follows the paper On-Manifold Preintegration for Real-Time Visual-Inertial Odometry (Forster et al.). It sets up visual-inertial odometry (VIO) as a factor graph, derives the IMU kinematics, builds the preintegrated measurements and their noise, shows how to handle bias changes cheaply, and ends with how to use all of it in GTSAM.

IMU Preintegration

What problems does it solve

Visual-inertial odometry attains highly accurate state estimates with nonlinear optimization: keep a window (or the whole history) of states and minimize the visual and inertial errors jointly. The problem is the IMU rate, typically 100 Hz to 1 kHz. A naive implementation adds a new state for every IMU measurement, and the number of variables grows so fast that real-time optimization quickly becomes infeasible.

The usual fix is to keep states only at selected keyframes and to integrate the IMU measurements between two keyframes into a single relative-motion constraint. Plain integration, however, needs an initial condition: the pose and velocity at the first keyframe. Every iteration of the optimizer changes that estimate, so the integration over all keyframe intervals would have to be repeated at every iteration.

Lupton and Sukkarieh showed that this repeated integration can be avoided by reparametrizing the relative motion constraint so that it no longer depends on the state at the first keyframe. That reparametrization is called IMU preintegration. Forster et al. carried it out properly on the rotation manifold SO(3), derived its noise covariance, and gave a first-order correction for bias changes, which is what makes it practical. It solves three problems at once:

1. Size of the problem. Hundreds of IMU samples between two keyframes become one factor between two states.
2. Repeated integration. The preintegrated measurement is computed once from the raw IMU data, independent of the keyframe states.
3. Bias changes. When the optimizer updates the bias estimate, the measurement is corrected with precomputed Jacobians instead of being re-integrated.

Visual-inertial state estimation as MAP

The state

The state of the system at time $i$ is the IMU orientation, position, velocity and biases:

The pose $(R_i, \mathbf{p}_i)$ belongs to SE(3), the velocity lives in a vector space, $\mathbf{v}_i \in \mathbb{R}^3$, and the biases are $\mathbf{b}_i = [\mathbf{b}^g_i, \mathbf{b}^a_i] \in \mathbb{R}^6$, where $\mathbf{b}^g_i, \mathbf{b}^a_i \in \mathbb{R}^3$ are the gyroscope and accelerometer bias.

Let $\mathcal{K}_k$ denote the set of all keyframes up to time $k$. We estimate the state of all keyframes:

The paper uses a structure-less approach, so the 3D landmarks are not variables of the optimization. The same formulation extends directly to estimating landmarks and the camera intrinsic and extrinsic calibration.

The measurements

The inputs are the camera and IMU measurements. $\mathcal{C}_i$ denotes the image measurements at keyframe $i$. The camera can observe several landmarks $l$ at time $i$, so $\mathcal{C}_i$ contains several image measurements $\mathbf{z}_{il}$; with slight abuse of notation we write $l \in \mathcal{C}_i$ when landmark $l$ is seen at time $i$.

$\mathcal{I}_{ij}$ denotes the set of IMU measurements acquired between two consecutive keyframes $i$ and $j$. Depending on the IMU rate and on how often keyframes are selected, it contains anything from a few to hundreds of measurements. The set of measurements collected up to time $k$ is

Factor graph and MAP estimation

The posterior of the variables $\mathcal{X}_k$ given the measurements $\mathcal{Z}_k$ and the prior $p(\mathcal{X}_0)$ is

Step (a) assumes the measurements are independent of each other. Step (b) applies the Markov property: an image measurement at time $i$ depends only on the state at time $i$, and the IMU measurements between $i$ and $j$ depend only on the states at $i$ and $j$.

The measurements are known, so they become parameters of the factors, and the unknowns are the only variables. This is a factor graph: a bipartite graph with one node per unknown state and one node per probability factor, with an edge whenever a factor involves a state. For VIO, every keyframe state carries vision factors, and consecutive keyframe states are connected by one IMU factor and one bias factor.

The MAP estimate $\mathcal{X}_k^\star$ maximizes the posterior, which is the same as minimizing the negative log-posterior. Under zero-mean Gaussian noise, the negative log-posterior is a sum of squared residuals:

where $\lVert \mathbf{r} \rVert^2_\Sigma = \mathbf{r}^T \Sigma^{-1} \mathbf{r}$. $\mathbf{r}_0$, $\mathbf{r}_{\mathcal{I}_{ij}}$ and $\mathbf{r}_{\mathcal{C}_{il}}$ are the residuals of the prior, the IMU and the camera measurements, and $\Sigma_0$, $\Sigma_{ij}$, $\Sigma_{\mathcal{C}}$ their covariances. A residual measures the mismatch between a measured quantity and the value predicted from the state. The rest of this post derives $\mathbf{r}_{\mathcal{I}_{ij}}$ and $\Sigma_{ij}$.

IMU model and motion integration

What an IMU measures

In one sentence each:

• The gyroscope measures the angular velocity of the IMU frame relative to an inertial frame, expressed in the IMU frame.
• The accelerometer measures the acceleration of the IMU relative to an inertial frame, minus gravity, expressed in the IMU frame.

Both are corrupted by additive white noise $\boldsymbol{\eta}$ and a slowly varying bias $\mathbf{b}$:

The pose of the IMU is $\lbrace R_{WB}, {}_W\mathbf{p}\rbrace$, which maps a point from the body (IMU) frame $B$ to the world frame $W$. The body frame is assumed to coincide with the IMU frame, and the camera pose in the body frame, $T_{BC}$, is known from calibration. ${}_B\boldsymbol{\omega}_{WB}(t) \in \mathbb{R}^3$ is the angular velocity of $B$ relative to $W$, expressed in $B$; ${}_W\mathbf{a}(t) \in \mathbb{R}^3$ is the acceleration of the sensor and ${}_W\mathbf{g}$ the gravity vector, both in world coordinates. We neglect the rotation of the earth, which amounts to treating $W$ as an inertial frame.

Why the minus gravity

An accelerometer does not measure total acceleration. It measures non-gravitational acceleration, also called proper acceleration or specific force:

That is where the minus sign comes from. A stationary IMU has $\mathbf{a} = \mathbf{0}$, so it measures $\mathbf{f} = -\mathbf{g}$. With a z-up world frame,

which is what an IMU reports when it is stationary with its z-axis pointing straight up (for example, a board lying level, component side up): $[0,\ 0,\ 9.81]^T\ \mathrm{m/s^2}$, plus bias. Intuitively, the table pushes the IMU up with a force that cancels gravity, and that push is what the accelerometer feels. An IMU in free fall has $\mathbf{a} = \mathbf{g}$ and measures zero.

The reading depends on how the sensor’s own axes are oriented, not on the world-frame convention. A tilted IMU spreads the same 9.81 across its axes, $\tilde{\mathbf{a}} = R_{WB}^T [0,\ 0,\ 9.81]^T$, which is how a static IMU estimates roll and pitch. An IMU whose z-axis points down, such as the forward-right-down body frame used with NED world frames in aerospace, reads $[0,\ 0,\ -9.81]^T$ at rest. In both cases the specific force points physically up, away from the Earth.

Kinematic model and integration

To infer motion from the IMU, use the kinematic model

where $(\cdot)^\wedge$ turns a vector into its skew-symmetric matrix. The first equation is the time derivative of a rotation whose angular velocity is expressed in the body frame (see the section Rotation as a continuous-time trajectory in SO(3) in Solà’s Quaternion kinematics for the error-state Kalman filter). The state at time $t + \Delta t$ follows by integration:

$\operatorname{Exp}(\boldsymbol{\phi})$ maps a rotation vector to a rotation matrix, and $\operatorname{Log}$ is its inverse. The rotation formula is exact only when the direction of $\boldsymbol{\omega}$ is constant over the interval, which is fine for the short intervals used next. Assuming ${}_W\mathbf{a}$ and ${}_B\boldsymbol{\omega}_{WB}$ are constant in $[t, t+\Delta t]$:

Now replace ${}_W\mathbf{a}$ and ${}_B\boldsymbol{\omega}_{WB}$ with the IMU model solved for the true quantities. Dropping the frame subscripts from here on, since the notation is unambiguous:

This is equation (31) of the paper. The velocity and position updates assume the orientation $R(t)$ is constant between two measurements, which is not an exact solution of $\dot{R} = R\,\boldsymbol{\omega}^\wedge$ when the rotation rate is non-zero. A high-rate IMU keeps this error small. The scheme is simple and easy to propagate uncertainty through; for slower IMUs, a higher-order integration method may be worth it.

$\boldsymbol{\eta}^{gd}$ and $\boldsymbol{\eta}^{ad}$ are the discrete-time noises. Their covariance depends on the sampling period and relates to the continuous-time noise spectral density by

The faster the IMU samples, the noisier each individual sample, but averaging over more samples per second keeps the integrated noise the same.

IMU preintegration on manifold

Integrating from one keyframe to the next

Assume the IMU is synchronized with the camera and provides measurements at discrete times $k$. Iterating equation (31) over all intervals between keyframes $k = i$ and $k = j$, with $\Delta t_{ij} = \sum_{k=i}^{j-1} \Delta t$ and the shorthand $(\cdot)_k = (\cdot)(t_k)$:

This already predicts the motion from $t_i$ to $t_j$, but it has the drawback from the introduction: it has to be recomputed whenever the linearization point at $t_i$ changes. A change in $R_i$ changes every $R_k$ for $k = i, \dots, j-1$, and with them every term of the sums.

Relative motion increments

To avoid that, move everything that depends on the state at $t_i$ to the left-hand side, and rotate into the frame of keyframe $i$. This defines relative motion increments whose right-hand sides are independent of the pose and velocity at $t_i$:

with $\Delta R_{ik} = R_i^T R_k$ and $\Delta \mathbf{v}_{ik} = R_i^T(\mathbf{v}_k - \mathbf{v}_i - \mathbf{g}\, \Delta t_{ik})$. This is equation (33) of the paper.

The rotation and velocity lines follow by multiplying the first two lines of the previous block by $R_i^T$. The position line takes a little more work. Write $\mathbf{a}^{\prime}_k = \tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \boldsymbol{\eta}^{ad}_k$ and use the definition of $\Delta \mathbf{v}_{ik}$ to replace each velocity, $\mathbf{v}_k = \mathbf{v}_i + \mathbf{g}\, \Delta t_{ik} + R_i \Delta \mathbf{v}_{ik}$:

With $n = j - i$ samples, $\Delta t_{ik} = (k - i)\Delta t$ and the gravity sum is $\Delta t^2 \big(\tfrac{n(n-1)}{2} + \tfrac{n}{2}\big) = \tfrac{1}{2} n^2 \Delta t^2 = \tfrac{1}{2}\Delta t_{ij}^2$. Moving the $\mathbf{v}_i$ and $\mathbf{g}$ terms to the left and multiplying by $R_i^T$ gives the position line, since $R_i^T R_k = \Delta R_{ik}$.

Unlike $\Delta R_{ij}$, neither $\Delta \mathbf{v}_{ij}$ nor $\Delta \mathbf{p}_{ij}$ is the true physical change in velocity or position. They are defined so that the right-hand side is independent of the state at time $i$ and of gravity, which means it can be computed directly from the IMU measurements between the two keyframes.

Two issues remain with equation (33):

1. The sums and products still depend on the bias estimate.
2. The right-hand side has a fairly intricate dependence on the measurement noise, which makes it awkward to write down a likelihood.

The rest of the derivation fixes both: assume the bias stays constant between two keyframes and correct it to first order later, and rewrite equation (33) so that the noise appears as a separate additive term.

For the first point, assume

Preintegrated IMU measurements

Equation (33) relates the states at keyframes $i$ and $j$ (left-hand side) to the measurements (right-hand side), so it is already a measurement model. To make the likelihood easy to write, isolate the noise.

Rotation. For a small $\delta\boldsymbol{\phi}$, $\operatorname{Exp}(\boldsymbol{\phi} + \delta\boldsymbol{\phi}) \approx \operatorname{Exp}(\boldsymbol{\phi})\operatorname{Exp}(J_r(\boldsymbol{\phi})\, \delta\boldsymbol{\phi})$, where $J_r$ is the right Jacobian of SO(3). Applying this to each factor of the product splits off the noise:

The identity $\operatorname{Exp}(\boldsymbol{\phi})\, R = R \operatorname{Exp}(R^T\boldsymbol{\phi})$ moves all the noise terms to the end of the product:

where the preintegrated rotation measurement is

Velocity and position. Substitute $\Delta R_{ik} \approx \Delta \tilde{R}_{ik} \operatorname{Exp}(-\delta\boldsymbol{\phi}_{ik}) \approx \Delta \tilde{R}_{ik}(I - \delta\boldsymbol{\phi}_{ik}^\wedge)$ into the velocity and position lines of equation (33), use $-\delta\boldsymbol{\phi}^\wedge \mathbf{a} = \mathbf{a}^\wedge \delta\boldsymbol{\phi}$, and drop the products of noise terms. Everything noise-free collects into the preintegrated velocity and position measurements

and the rest into the noise terms $\delta\mathbf{v}_{ij}$ and $\delta\mathbf{p}_{ij}$, given in the noise propagation section below.

The measurement model. Plugging the three expressions back into equation (33) gives

This is equation (38) of the paper, and it has exactly the form we want: measurement = function of the state “plus” noise.

• The left-hand side, $\Delta \tilde{R}_{ij}$, $\Delta \tilde{\mathbf{v}}_{ij}$ and $\Delta \tilde{\mathbf{p}}_{ij}$, is computed directly from the raw IMU measurements and the bias estimate.
• The first term on the right depends only on the state variables at $i$ and $j$.
• $\delta\boldsymbol{\phi}_{ij}$, $\delta\mathbf{v}_{ij}$ and $\delta\mathbf{p}_{ij}$ are the noise.

With this form the log-likelihood is straightforward. If $\delta\mathbf{v}_{ij}$ and $\delta\mathbf{p}_{ij}$ are zero-mean Gaussian, the negative log-likelihood of the last two lines is a quadratic function. The same holds for the rotation if $\delta\boldsymbol{\phi}_{ij}$ is zero-mean Gaussian, with the error measured in the tangent space through $\operatorname{Log}$.

Noise propagation

Stack the preintegration noise into

To first order, the three components are

These express $\boldsymbol{\eta}^\Delta_{ij}$ as a linear function of the IMU noise $\boldsymbol{\eta}^d_k \doteq [\boldsymbol{\eta}^{gd}_k, \boldsymbol{\eta}^{ad}_k]$. So once the covariance $\Sigma_\eta$ of $\boldsymbol{\eta}^d_k$ is known from the IMU datasheet, $\Sigma_{ij}$ follows by linear propagation.

In practice the propagation is done incrementally, one IMU sample at a time, so the covariance is ready the moment the next keyframe arrives. Each new sample updates the noise as $\boldsymbol{\eta}^\Delta_{i,k+1} = A_k\, \boldsymbol{\eta}^\Delta_{ik} + B_k\, \boldsymbol{\eta}^d_k$, starting from $\Sigma_{ii} = \mathbf{0}_{9 \times 9}$:

This is an EKF-style covariance prediction, but on the relative increments instead of on the absolute state.

Incorporating bias updates

So far the bias $\lbrace \bar{\mathbf{b}}^a_i, \bar{\mathbf{b}}^g_i\rbrace$ used during preintegration between $k = i$ and $k = j$ was assumed correct and fixed. During optimization, though, the bias estimate changes by a small $\delta\mathbf{b}$. Recomputing the preintegrated measurements at every change would bring back the repeated integration we wanted to avoid, so instead, for a bias update $\mathbf{b} \leftarrow \bar{\mathbf{b}} + \delta\mathbf{b}$, use a first-order expansion:

The Jacobians are evaluated at $\bar{\mathbf{b}}_i$, remain constant, and can be precomputed during preintegration, accumulating one term per IMU sample:

The rotation does not depend on the accelerometer bias, which is why it has only a gyroscope Jacobian. If the bias drifts far from $\bar{\mathbf{b}}_i$, the linearization degrades; implementations then re-integrate with the new bias as the linearization point.

Preintegrated IMU factors

With the measurement model of equation (38), Gaussian noise with covariance $\Sigma_{ij}$ (to first order), and the bias correction above, the IMU residual $\mathbf{r}_{\mathcal{I}_{ij}} \doteq [\mathbf{r}_{\Delta R_{ij}}^T,\ \mathbf{r}_{\Delta \mathbf{v}_{ij}}^T,\ \mathbf{r}_{\Delta \mathbf{p}_{ij}}^T]^T \in \mathbb{R}^9$ is

Every quantity with a tilde or a bar is computed once from the IMU data. When the optimizer evaluates the factor, it only plugs in the current states $\mathbf{x}_i$, $\mathbf{x}_j$ and the bias change: no re-integration, whatever the linearization point. The paper also gives the analytic Jacobians of these residuals with respect to the states, which a Gauss-Newton or Levenberg-Marquardt solver needs.

Bias model

Biases are slowly time-varying, so they are modeled as a Brownian motion, that is, integrated white noise:

Integrating over the interval $[t_i, t_j]$ between two consecutive keyframes gives

where the discrete noises have zero mean and covariance $\Sigma^{bgd} \doteq \Delta t_{ij} \operatorname{Cov}(\boldsymbol{\eta}^{bg})$ and $\Sigma^{bad} \doteq \Delta t_{ij} \operatorname{Cov}(\boldsymbol{\eta}^{ba})$. Note that the variance grows linearly with the time between keyframes: the longer the gap, the more the bias is allowed to drift. This adds one more term per pair of consecutive keyframes to the MAP cost:

Usage in GTSAM

GTSAM implements all of the above in gtsam/navigation. The pieces map one to one onto the sections of this post:

ImuFactor connects pose and velocity at $i$ and $j$ and the bias at $i$; the bias evolution is then added separately, typically with a BetweenFactor<imuBias::ConstantBias>. CombinedImuFactor connects the biases at both $i$ and $j$ and propagates the bias random walk inside the same covariance, which is the usual choice.

A minimal loop between two keyframes looks like this:

#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

using gtsam::symbol_shorthand::B;  // bias
using gtsam::symbol_shorthand::V;  // velocity
using gtsam::symbol_shorthand::X;  // pose

// Z-up navigation frame: gravity points along -z.
auto params = gtsam::PreintegrationCombinedParams::MakeSharedU(9.81);
params->accelerometerCovariance = gtsam::I_3x3 * std::pow(accel_noise_density, 2);
params->gyroscopeCovariance     = gtsam::I_3x3 * std::pow(gyro_noise_density, 2);
params->biasAccCovariance       = gtsam::I_3x3 * std::pow(accel_bias_random_walk, 2);
params->biasOmegaCovariance     = gtsam::I_3x3 * std::pow(gyro_bias_random_walk, 2);
params->integrationCovariance   = gtsam::I_3x3 * 1e-8;
params->body_P_sensor = body_T_imu;  // optional, if the IMU is not the body frame

gtsam::imuBias::ConstantBias bias_i;  // current estimate, the linearization point b_bar
gtsam::PreintegratedCombinedMeasurements pim(params, bias_i);

// Between keyframe i and keyframe j, feed every IMU sample.
for (const auto& m : imu_samples_between_i_and_j) {
  pim.integrateMeasurement(m.acc, m.gyro, m.dt);
}

// Optional: predict state j from state i, e.g. as the initial guess.
gtsam::NavState state_j_guess = pim.predict(gtsam::NavState(pose_i, vel_i), bias_i);

gtsam::NonlinearFactorGraph graph;
graph.add(gtsam::CombinedImuFactor(X(i), V(i), X(j), V(j), B(i), B(j), pim));

// After optimizing, restart preintegration from the new bias estimate.
pim.resetIntegrationAndSetBias(optimized_bias_j);

A few practical notes:

• Noise parameters are the continuous-time densities from the IMU datasheet or an Allan-variance calibration (for example with Kalibr): accelerometer noise density in $\mathrm{m/s^2/\sqrt{Hz}}$, gyroscope noise density in $\mathrm{rad/s/\sqrt{Hz}}$, and the bias random walks. GTSAM divides by $\Delta t$ internally, as in the discrete-noise relation above. Datasheet values are usually inflated by a factor of a few in practice to absorb unmodeled effects.
• Gravity direction. MakeSharedU is for a z-up navigation frame (gravity along $-z$), MakeSharedD for z-down frames such as NED. Getting this wrong is the classic IMU-factor bug: a stationary IMU then looks like it is accelerating at $2g$.
• Measurement order. integrateMeasurement expects the specific force from the accelerometer and the angular rate from the gyroscope, in the IMU frame, exactly as the sensor reports them.
• Bias linearization point. The factor stores the Jacobians from the bias-update section and applies the first-order correction while the optimizer runs. Calling resetIntegrationAndSetBias with the latest estimate before integrating the next interval keeps $\delta\mathbf{b}$ small, so the linearization stays accurate.
• Tangent versus manifold preintegration. GTSAM builds with GTSAM_TANGENT_PREINTEGRATION on by default, which preintegrates in the tangent space of NavState rather than with the product of rotations used in the paper (ManifoldPreintegration). The factor interface is identical, and both are accurate for typical keyframe intervals.
• Fixed-lag smoothing. For real-time use, these factors are usually combined with vision factors in an IncrementalFixedLagSmoother or ISAM2, which only relinearizes the parts of the graph that changed.

Reference

1. C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza. On-Manifold Preintegration for Real-Time Visual-Inertial Odometry. IEEE Transactions on Robotics, 2017.
2. T. Lupton, S. Sukkarieh. Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions. IEEE Transactions on Robotics, 2012.
3. J. Solà. Quaternion kinematics for the error-state Kalman filter. arXiv:1711.02508, 2017.
4. GTSAM ImuFactor and CombinedImuFactor.