Galilean IMU preintegration with a left-invariant error

This guide follows the construction in Left-Invariant Galilean IMU Preintegration in Inertial and Rotating Frames: a direct-product preintegration state, exact zero-order-held Galilean increments, standard left-invariant error, adjoint covariance transport, right-applied bias correction, and exact constant-rate rotating-frame prediction.

The method retains the exact held-input Galilean composition introduced by Delama et al., but replaces their augmented tangent-semidirect-product model with $\mathrm{Gal}(3)\times\mathbb R^6$ and the physical accelerometer and gyroscope biases. The $W\phi(X)U$ endpoint relation and constant-rate rotating-frame structure follow Barrau--Bonnabel and Brossard et al.; the direct-product bias model, left-invariant covariance, and right-applied correction are the construction developed in the companion paper.

For background, see the Gal3 guide, the Gal3ImuEKF guide, the NavState guide, and the standard ImuFactor guide. The companion NEES comparison evaluates all four GTSAM preintegration backends under identical high-dynamic IMU samples.

1. Conventions: left-invariant error means an update on the right¶

We fix the convention before introducing the dynamics. For a matrix Lie group $G$ with Lie algebra $\mathfrak g$, GTSAM uses

The group error $\hat X^{-1}X$ is left-invariant: replacing both arguments by $AX$ and $A\hat X$ leaves it unchanged. Thus, throughout this guide, left-invariant error and right perturbation describe the same convention.

Delama et al.'s related construction instead uses the navigation error $\Upsilon\hat\Upsilon^{-1}$, which is right-invariant, together with a left-applied first-order correction. Here we use the direct-product state and the left-invariant error $\hat\Upsilon^{-1}\Upsilon$, with right perturbations throughout.

2. Physical state¶

The preintegration state consists of the accumulated Galilean motion and the physical six-axis IMU bias:

Here $\Upsilon_k=(\Delta R_k,\Delta v_k,\Delta p_k,\Delta t_k)$ is the motion being preintegrated. The second component is exactly gtsam::imuBias::ConstantBias; its vector representation stores accelerometer bias before gyroscope bias. There are no additional bias coordinates. Bias perturbations and bias-Jacobian columns retain this ordering throughout.

3. The Galilean motion component¶

A Galilean element stores rotation, velocity, position, and elapsed time:

Matrix multiplication gives

The term $t_2v_1$ is the defining Galilean coupling: during the second interval, the velocity accumulated by the first interval advances position. The inverse is

Using GTSAM’s tangent ordering, an algebra element is

For an integrated IMU increment, $\omega$ is an angle, $\nu$ a velocity increment, $\rho$ a position increment, and $\alpha$ an elapsed time. Before multiplication by $\Delta t$, the corresponding rate vector has units $({\rm rad/s}, {\rm m/s^2}, {\rm m/s}, 1)$.

Exponential, Jacobians, and adjoint¶

Let $J_L(\omega)$ be the $SO(3)$ left Jacobian and $\Gamma_2(\omega)$ its second integral. Then

The $\alpha\Gamma_2(\omega)\nu$ term produces the familiar $\tfrac12a\Delta t^2$ when rotation is zero. For the complete 10D algebra, define the right Jacobian $J_R(x)$ by

This is the Jacobian required when both the state perturbation and the increment are applied on the right. It is related to the complete Galilean left Jacobian by

For $X=(R,v,p,t)$, the adjoint in the ordering $(\omega,\nu,\rho,\alpha)$ is

We write $\operatorname{ad}_x y=[x,y]$ for the algebra adjoint and use the identities $\operatorname{Ad}_{X^{-1}}=\operatorname{Ad}_X^{-1}$ and $\operatorname{ad}_x y=-\operatorname{ad}_y x$.

4. Mean preintegration¶

The measured input is the physical six-axis IMU sample supplied to GTSAM’s integrateMeasurement(measuredAcc, measuredOmega, deltaT) interface:

It follows the same accelerometer-then-gyroscope ordering as the bias. Holding this sample constant over an interval of duration $h$, its bias-corrected Galilean body rate is

The exact discrete dynamics for this held input are

The bias is constant inside preintegration because the three-way IMU factor uses a separate factor for bias evolution. No bias-process input is part of $\mathcal L$. For a measured sample $\tilde u_k=(\tilde a_k,\tilde\omega_k)$ and linearization bias $\hat\beta_k$, let

Starting with $\hat\Upsilon_{ii}=I$, the preintegrated mean is updated on the right:

The group law automatically performs all four component updates. In particular, old velocity advances position by $h\,\Delta v_{ik}$, while the exponential contributes the rotation-aware acceleration integral. No separate Euler position update is needed.

Gal(3) is internal to preintegration; the optimized keyframe states remain $X_i,X_j\in\mathrm{SE}_2(3)$. NavState stores and retracts it as $(R,p,v)$, and we define

and project the corrected $\Upsilon_{ij}$ to the body-frame increment

The inertial endpoint prediction is exactly

This $W\phi(X)U$ separation is important: gravity acts in the navigation frame on the left, autonomous coasting advances the existing position, and bias-corrected IMU motion acts in the body/local frame on the right.

5. Direct-product state and propagation¶

The preintegration state is the direct-product Lie group $\mathcal G=\mathrm{Gal}(3)\times(\mathbb R^6,+)$. Its coordinates $\xi=(\Upsilon,\beta)$ contain the Galilean state and the six physical IMU biases. Multiplication and inversion are

For an input held constant during a sample of duration $h$, propagate the Galilean component by its body-coordinate increment and hold the bias fixed:

The bias-corrected nominal input is $u^\circ=\tilde u-\hat\beta$, and we write $q^\circ=q(\tilde u,\hat\beta)$. The derivative of $q$ with respect to the corrected physical input maps GTSAM’s $(a,\omega)$ order into Gal3’s $(\omega,\nu,\rho,\alpha)$ coordinates:

The fixed zero position-rate and unit clock-rate coordinates have zero derivative. This matrix supplies the six physical bias and noise columns used in the error linearization below.

Delama et al.'s related model augments the bias to the full Galilean algebra and uses a tangent-semidirect-product construction. Here the direct product closes the propagation and error calculation on the physical six-dimensional bias space.

6. Left-invariant error and discrete dynamics¶

The direct-product error is the estimated state inverse multiplied by the true state:

It is left-invariant because $(Z\hat\xi)^{-1}(Z\xi)=\hat\xi^{-1}\xi$. Its local coordinates are

The navigation component is GTSAM’s right-retracted local coordinate. Linearizing the physical propagation in these coordinates gives the transition and noise matrices below.

Exact discrete linearization¶

Adopt the six-dimensional measurement convention

The true bias is constant during this preintegration step. For one nominal increment define

The true corrected physical input is $\tilde u-\hat\beta-\delta\beta-\eta$. By the definition of the complete Galilean right Jacobian, its increment satisfies

Write $\mathcal E=\hat\Upsilon^{-1}\Upsilon=\operatorname{Exp}(e)$. After one mean update,

Transporting the old right perturbation through $V$ and retaining first-order terms gives

Therefore, with $\epsilon=(e,\delta\beta)\in\mathbb R^{16}$,

where

The upper-left block is not generally the identity: $\operatorname{Ad}_{V^{-1}}$ transports a body/local error through the next increment. The upper-right block maps the physical GTSAM-ordered bias error directly into Gal3 coordinates through $C$; no virtual bias columns are introduced.

For an input held constant during the sample, the corresponding continuous-time error matrix is

The $-\operatorname{ad}_{q^\circ}$ block describes transport of the body-coordinate error; the $-E$ block describes the effect of bias error on the corrected input.

Covariance propagation¶

Let $\Sigma_k\in\mathbb R^{16\times16}$ be the covariance of $(e,\delta\beta)$. In GTSAM order, the continuous-time IMU noise density is

where $Q_d$ is the covariance of the sampled rate noise. The first-order augmented propagation is

Only physical accelerometer and gyroscope noise enters this model. If a caller already supplies per-sample rather than continuous-time covariances, the $1/h$ conversion must not be applied a second time.

The three-way IMU factor does not itself constrain temporal bias evolution. It uses the navigation uncertainty conditioned on the bias linearization value, while a separate bias between-factor supplies the chosen random-walk model. Therefore initialize $\delta\beta=0$ and propagate the conditional $10\times10$ navigation covariance directly:

The deterministic $10\times6$ bias sensitivity derived next accounts for changing the optimizer’s bias away from its linearization value; it is not part of this conditional covariance. A combined-bias factor could extend the input with a physical six-dimensional bias random walk and retain the full augmented covariance, without introducing virtual coordinates.

The augmented covariance above belongs to the standard left-invariant error of the direct-product group used in GTSAM. It must not be identified directly with Delama et al.'s semidirect-product covariance, whose navigation and bias components use different frames and coordinates. For the conditioned navigation covariance consumed by the ordinary factor, the descriptions agree after the corresponding frame conversion.

7. Bias correction must also act on the right¶

Preintegration is performed once at a linearization bias $\hat\beta=(\hat b_a,\hat b_\omega)$ in GTSAM order. When optimization proposes $\hat\beta+\delta\beta$, recomputing every IMU sample would be wasteful. Define $J_k\in\mathbb R^{10\times6}$ directly by

This correction is on the right, matching the retraction convention. For $V_k=\operatorname{Exp}(x_k)$ and $C_k=J_R(x_k)hE\in\mathbb R^{10\times6}$, transport of the old correction through the new increment gives

The minus sign follows from differentiating $\tilde u-\beta$. The same recursion is obtained by accumulating the upper-right block of $A_{\mathrm{LI}}$: the direct-product transition exposes the physical bias-to-navigation sensitivity directly. Apply it as

The six columns of $J_k$ are ordered $(b_a,b_\omega)$, exactly like ConstantBias::vector(). There is no intermediate $10\times10$ virtual-bias Jacobian and no final column permutation. For the standard Galilean PIM, bias evolution remains the responsibility of a separate factor.

8. From the Galilean uncertainty to a GTSAM factor¶

The factor consumes a nine-dimensional NavState residual rather than the ten-dimensional Gal3 local error. In their respective tangent orderings,

Define the required selection and permutation when constructing the factor:

The final column removes elapsed time, which is known exactly from the measurement timestamps. This projection applies to local errors, covariances, and Jacobians. The mean Galilean element is converted separately to NavState as $(R,p,v)$.

Let $\Sigma_e\in\mathbb R^{10\times10}$ be the conditional Galilean covariance propagated above. The factor’s NavState residual covariance is

Likewise, the first-order bias sensitivity of the local navigation error is

For the mean, first apply the Galilean bias correction and then construct

The known gravity and elapsed-time terms combine this delta with $X_i$ to form predictedState_j. The residual used by the existing GTSAM preintegration machinery is

In the C++ interface this is NavState::logmap; the argument order is intentional because GTSAM’s IMU residual asks how the measured state retracts to the prediction. This factor residual is independent of the optimization chart selected by GTSAM_NAVSTATE_EXPMAP.

Conceptually this is a three-variable relation $(X_i,X_j,\beta_i)$. The GalileanImuFactor alias uses ImuFactorT, so each NavState is exposed as separate pose and velocity keys and the concrete graph factor has five keys. In both views there is one bias variable for the entire interval. Bias evolution remains a separate factor, exactly as for the standard ImuFactor.

When bias evolution and state--bias correlation should be modeled inside the IMU factor, use PreintegratedCombinedMeasurementsG with GalileanCombinedImuFactor. The Combined PIM retains the same Galilean mean while propagating a public $15\times15$ covariance over $(R,p,v,b_a,b_\omega)$. The Gal(3) clock coordinate is deterministic and is projected out exactly. The resulting six-way factor adds the six bias-random-walk residuals to the nine navigation residuals.

9. Exact prediction in a rotating navigation frame¶

Let $\omega_n$ be the constant angular velocity of the navigation frame, expressed in navigation-frame coordinates, and let $\Omega=[\omega_n]_\times$. In the $(R,v,p)$ display order, the continuous dynamics are

GTSAM stores the same state as NavState(R,p,v). Define the mutually inverse transported-velocity maps in that order by

For $\theta_{ij}=-\omega_n\Delta t_{ij}$, form the exact $SO(3)$ kernels

and the rotating-frame world increment, now written directly in NavState order,

The complete endpoint prediction preserves the same left-linear backbone:

Writing $U_{ij}=(\Delta R,\Delta p,\Delta v)$ and $W^\Omega_{ij}=(A,p_W,v_W)$ in GTSAM order makes the implementation explicit:

This is the companion paper’s extension of Brossard et al.'s transported-velocity construction to the Galilean body-frame increment. It gives an exact constant-rate rotating-frame transition: Coriolis and centrifugal terms arise from the lift, group composition, and projection rather than an appended acceleration correction. The body-frame increment $U_{ij}$, its covariance, and its bias correction are unchanged; only endpoint prediction and its Jacobians depend on $\omega_n$. In Python, enable this path with params.setOmegaCoriolis(omega_n). Omitting it (or setting exactly zero) selects the inertial prediction.

10. Intended use¶

A user-facing workflow mirrors the existing IMU factors:

1. Construct preintegration parameters and a PreintegratedImuMeasurementsG at the current bias estimate. When the navigation frame rotates, call setOmegaCoriolis with its constant angular velocity in navigation-frame coordinates.

2. Integrate each accelerometer/gyroscope sample. The public API and all six-dimensional quantities use GTSAM order $(a,\omega)$; the lift places them in Gal3 order internally. Each call advances the Galilean mean on the right and propagates its conditional covariance.

3. Construct a GalileanImuFactor between the two pose/velocity pairs and the interval’s bias key. During optimization, use the right-applied first-order bias correction rather than reintegrating immediately.

4. Add a separate factor between consecutive bias keys when bias random-walk evolution is part of the model. Alternatively, construct PreintegratedCombinedMeasurementsG and a GalileanCombinedImuFactor to model the second bias key, its random walk, and state--bias correlation inside one factor.

In schematic C++ form:

PreintegratedImuMeasurementsG pim(params, biasHat);
for (const ImuSample& sample : samples) {
  pim.integrateMeasurement(sample.acceleration, sample.angularRate,
                           sample.deltaT);
}
graph.emplace_shared<GalileanImuFactor>(
    X(i), V(i), X(j), V(j), B(i), pim);

The Combined alternative has the corresponding form:

PreintegratedCombinedMeasurementsG combinedPim(combinedParams, biasHat);
// Integrate the same samples as above.
graph.emplace_shared<GalileanCombinedImuFactor>(
    X(i), V(i), X(j), V(j), B(i), B(j), combinedPim);

The API is deliberately familiar; the essential differences are internal geometric choices. The executable Python example below follows the same sequence. It predicts a consistent endpoint only to demonstrate factor construction; in a real graph, $X_j$ is an optimizer variable rather than the prediction itself.

References¶

• Left-Invariant Galilean IMU Preintegration in Inertial and Rotating Frames (companion paper): the direct-product model implemented and derived here.

• M. Brossard, A. Barrau, P. Chauchat, and S. Bonnabel, Associating Uncertainty to Extended Poses for On Lie Group IMU Preintegration With Rotating Earth, IEEE Transactions on Robotics, 2022.

• G. Delama, A. Fornasier, R. Mahony, and S. Weiss, Equivariant IMU Preintegration with Biases: a Galilean Group Approach, IEEE Robotics and Automation Letters, 2025.

• gtsam::Gal3: group operations, exponential and logarithmic maps, and adjoints.

• Gal3ImuEKF: Galilean state propagation in an invariant EKF.

• NavState: GTSAM’s navigation manifold and local-coordinate convention.

• ImuFactor: standard preintegration and factor-graph usage.