Galilean IMU preintegration with a left-invariant error

This guide develops the left-invariant Galilean IMU preintegration used by GTSAM: a direct-product symmetry, compatible left actions, exact zero-order-held Galilean increments, adjoint covariance transport, right-applied bias correction, and exact constant-rate rotating-frame prediction.

The construction is motivated by Delama, Fornasier, Mahony, and Weiss, Equivariant IMU Preintegration with Biases: a Galilean Group Approach, but deliberately uses a different symmetry and perturbation convention. It closes on the physical six-dimensional IMU input and bias in accelerometer-then-gyroscope order, and it induces the same left-invariant, component-wise right-retracted endpoint error used by GTSAM.

For background, see the EquivariantFilter guide, 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 motivating construction instead uses the navigation error $\Upsilon\hat\Upsilon^{-1}$, which is right-invariant, together with a left-applied first-order correction. Reversing only the error while retaining their right action would not produce an invariant EqF error; the direct-product left action introduced in Section 5 is what induces $\hat\Upsilon^{-1}\Upsilon$.

Do not conflate multiplication sides. ActionType::Left below describes how the symmetry moves the physical state and input. GTSAM’s EquivariantFilter prediction still composes the lifted increment on the right, while its measurement update composes the innovation on the left. The side of a symmetry action, the side of a group increment, and the invariance of an error are related choices, but they are not synonyms.

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. The equivariant-filter construction¶

GTSAM’s EquivariantFilter<M, Symmetry> separates the physical state manifold $M$ from a symmetry group $G$ that moves points of $M$. A complete model also supplies an action on the input space and a lift that reconstructs the dynamics on $G$. For this factor the template roles are

Direct-product symmetry group¶

Choose the direct product

where the second factor is the additive physical bias space in GTSAM order. For $Y_1=(\Gamma_1,c_1)$ and $Y_2=(\Gamma_2,c_2)$,

This group has the same dimension as $\mathcal M$, but its role is different: $\mathcal M$ contains physical states, whereas $\mathcal G_L$ supplies transformations of those states.

Left action on the state¶

Define

It is a left action because

In GTSAM terminology the corresponding Symmetry derives from GroupAction and declares ActionType::Left; its call signature is operator()(group, state). Fix the reference state

The orbit map used by EquivariantFilter is especially simple:

Consequently the lifted group estimate and the physical estimate have identical coordinates,

Left action on the input¶

The compatible input action is

It is also a left action. The Galilean component $\Gamma$ acts trivially on the physical IMU input, while $c=(c_a,c_\omega)$ translates it in the same GTSAM order as the bias. Consequently

and direct substitution verifies system equivariance:

The EquivariantFilter maps an input to the reference orbit by applying its InputOrbit to the inverse group estimate. Here

Thus the input at the EqF origin is exactly the bias-corrected body/local input; no adjoint transport is required.

Lift and reconstruction¶

Use the right-trivialized lift expected by GTSAM’s prediction path,

The direct-product exponential gives

The lift is also where the two coordinate orderings meet. For the linearization below, record its constant derivative with respect to a physical GTSAM-ordered six-vector:

This single matrix places angular rate and acceleration into Gal3’s internal $(\omega,\nu,\rho,\alpha)$ order. It is a derivative of the lift, not a second representation of the bias; the fixed zero position-rate and unit clock-rate coordinates have zero derivative.

The lifted state is propagated exactly as in EquivariantFilter::predictWithJacobian, by composing the increment on the right and then returning through the reference orbit:

For a left state action, this reconstruction equation is not $\phi_L(\operatorname{Exp}(\lambda h),\xi)$, which would put the Galilean increment on the wrong side. The lift is invariant under the simultaneous state and input actions:

Relation to Delama et al. Their construction closes its model over the full Galilean algebra: its input and bias coordinates lie in $\mathfrak{gal}(3)$, including virtual coordinates, and its symmetry is the semidirect product $\mathcal G_R=\mathrm{Gal}(3)\ltimes\mathfrak{gal}(3)$. It uses the right actions

$$> \phi_R((\Upsilon,b),(C,\gamma)) > =(\Upsilon C,\operatorname{Ad}_{C^{-1}}(b-\gamma)),\qquad > \psi_R((w,\tau),(C,\gamma)) > =(\operatorname{Ad}_{C^{-1}}(w-\gamma),\operatorname{Ad}_{C^{-1}}\tau). >$$

(34)

That coherent pairing induces the invariant error $X\hat X^{-1}$. The adjoint in their input action does not preserve the six-dimensional physical IMU subspace by itself, which motivates completing the input and bias to the full algebra. The direct-product construction used here needs no adjoint transport: it is closed on the physical $\mathbb R^6$ input and bias spaces, while the lift supplies the fixed Gal3 coordinates. The group and both actions change together; this is not merely a reversal of the error.

6. Equivariant error and left-invariant discrete dynamics¶

For a left action, the EqF error is obtained by acting on the true state with the inverse group estimate:

This error is invariant under the common left action: replacing $\xi$ by $\phi_L(Y,\xi)$ and $\hat Y$ by $Y\hat Y$ leaves $\mathsf e$ unchanged. Because the reference orbit is the identity, its normal coordinates are simply

These are exactly the error coordinates in which EquivariantFilter::errorCovariance() is interpreted: a tangent vector at the fixed reference $\xi^\circ$. The navigation component is also exactly GTSAM’s right-retracted local coordinate.

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.

Using the generic filter template literally. For an input held constant during the sample, the continuous-time error matrix is

$$> A_c=\begin{bmatrix}-\operatorname{ad}_{q^\circ}&-E\\0&0\end{bmatrix}, > \qquad \operatorname{Exp}(A_c h)=A_{\mathrm{LI}}. >$$

(44)

The current EquivariantFilter automatic path rejects left actions because $D\phi_0D\lambda$ alone omits the $-\operatorname{ad}_{q^\circ}$ transport associated with a left action and a right-composed prediction. A literal filter implementation must call predictWithJacobian with the continuous-time $A_c$; it must not pass the already-discrete $A_{\mathrm{LI}}$, which that method would discretize again. The GalileanImuFactor itself does not instantiate EquivariantFilter; the template terminology specifies the geometry used by its preintegration.

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 direct-product, left-action EqF error 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 EqF 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::localCoordinates; the argument order above is intentional because GTSAM’s IMU residual asks how the measured state retracts to the prediction.

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 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¶

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

• EquivariantFilter: GTSAM’s state, symmetry, orbit, input action, lift, and equivariant-error template vocabulary.

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