Galilean IMU preintegration: Logmap NEES and bias correction

This notebook compares GTSAM’s Manifold, Tangent, Lie-group, and Galilean IMU preintegration backends under identical measurements and noise samples. Every statistical experiment uses the IMU factor’s default $SE_2(3)$ Logmap residual,

e=X_j.\operatorname{logmap}(\widehat X_j),

(1)

so the comparison isolates preintegration means and covariances rather than changing the residual chart. The final experiment tests the right-applied first-order bias correction used by the paper’s direct-product Galilean model, alongside the corresponding correction for every other backend, directly against complete reintegration.

Here Galilean denotes the companion paper’s $\mathrm{Gal}(3)\times\mathbb R^6$ construction. It retains Delama et al.'s held-input Galilean composition and uses Brossard et al.'s endpoint and rotating-frame structure, but its physical bias model, left-invariant covariance, and right correction are the paper’s direct-product formulation.

The visible cells contain assumptions, scenario parameters, experiment calls, and conclusions. Mechanical Monte Carlo and plotting code lives in galilean_imu_factor_nees.py; plot cells are collapsed but expandable.

# Install GTSAM and fetch the notebook helper when running in Colab.
from pathlib import Path
import sys
import urllib.request

try:
    import google.colab
    %pip install --quiet gtsam-develop
    helper_path = Path("galilean_imu_factor_nees.py")
    if not helper_path.exists():
        urllib.request.urlretrieve(
            "https://raw.githubusercontent.com/borglab/gtsam/develop/"
            "gtsam/navigation/doc/galilean_imu_factor_nees.py",
            helper_path,
        )
except ImportError:
    candidates = [Path.cwd(), Path.cwd() / "gtsam/navigation/doc"]
    helper_directory = next(
        path for path in candidates
        if (path / "galilean_imu_factor_nees.py").exists()
    )
    sys.path.insert(0, str(helper_directory))

import numpy as np
from IPython.display import Markdown, display
import plotly.io as pio

import gtsam
import galilean_imu_factor_nees as nees

np.set_printoptions(precision=4, suppress=True)
pio.renderers.default = "notebook_connected"
nees.assert_logmap_default()
BACKENDS = nees.BACKENDS
STATE_I = gtsam.NavState()
ZERO_BIAS = gtsam.imuBias.ConstantBias()

1. Reading the experiments¶

For residual $e\in\mathbb R^d$ and predicted covariance $P$ ,

\operatorname{NEES}=e^\mathsf{T}P^{-1}e.

(2)

A consistent model has expected NEES $d$ . The reported green bands are exact 95% intervals for the sampled mean. Physical endpoint errors are reported separately because changing coordinates cannot repair deterministic integration error.

2. One-second inertial stress test¶

DURATION = 1.0
ACCELERATION = np.array([2.0, -1.0, 0.5])
ANGULAR_VELOCITY = np.array([1.2, -0.8, 2.0])
ACCELEROMETER_SIGMAS = np.array([0.03, 0.04, 0.05])
GYROSCOPE_SIGMAS = np.array([0.01, 0.015, 0.02])
SAMPLE_PERIODS = np.array([0.1, 0.05, 0.025, 0.0125])

PARAMS = nees.make_params(ACCELEROMETER_SIGMAS, GYROSCOPE_SIGMAS)
TRUTH, deterministic = nees.deterministic_convergence(
    ACCELERATION, ANGULAR_VELOCITY, DURATION,
    SAMPLE_PERIODS, PARAMS, STATE_I,
)

The three established backends use the same piecewise update for this trajectory and converge as the timestep shrinks. Galilean composition evaluates each held-input increment with the coupled exponential.

INERTIAL_DT = 0.05
INERTIAL_TRIALS = 3_000
INERTIAL_SEED = 2231

inertial = nees.run_inertial_nees(
    ACCELERATION, ANGULAR_VELOCITY, DURATION,
    INERTIAL_DT, INERTIAL_TRIALS, INERTIAL_SEED,
    ACCELEROMETER_SIGMAS, GYROSCOPE_SIGMAS,
    PARAMS, STATE_I, TRUTH,
)
nees.validate_finite(inertial)
display(Markdown(nees.inertial_table(inertial)))
print(nees.interval_text(inertial))

Expected 95% interval for mean NEES: [8.849, 9.152]

The Logmap residual does not hide finite-rate integration error. Under deliberately stressful 20 Hz simultaneous rotation and acceleration, the established backends remain overconfident; Galilean preintegration removes most of the held-input mean error.

3. Powered ascent in a rotating Earth frame¶

The next benchmark models four seconds of a high-power sounding-rocket ascent: constant 12 g measured specific force, a modest pitch rate, and Earth rotation at a representative launch-site latitude. Every backend is evaluated with omegaCoriolis both omitted and correctly specified.

EARTH_ANGULAR_SPEED = 7.292115e-5
SPACEPORT_LATITUDE = np.deg2rad(32.99)
EARTH_RATE = EARTH_ANGULAR_SPEED * np.array([
    0.0, np.cos(SPACEPORT_LATITUDE), np.sin(SPACEPORT_LATITUDE)
])
STANDARD_GRAVITY = 9.80665
ROTATING_GRAVITY = np.array([0.0, 0.0, -STANDARD_GRAVITY])
ROTATING_ACCELERATION = np.array([0.0, 0.0, 12.0 * STANDARD_GRAVITY])
ROTATING_ANGULAR_VELOCITY = EARTH_RATE + np.array([0.0, -0.04, 0.0])
ROTATING_DURATION = 4.0
ROTATING_DT = 0.05
ROTATING_STEPS = round(ROTATING_DURATION / ROTATING_DT)
ROTATING_TRIALS = 3_000
ROTATING_SEED = 2232
ROCKET_ACCELEROMETER_SIGMAS = ACCELEROMETER_SIGMAS.copy()
ROCKET_GYROSCOPE_SIGMAS = np.array([5e-4, 7.5e-4, 1e-3])

ROTATING_PARAMS = {
    "Not specified": nees.make_params(
        ROCKET_ACCELEROMETER_SIGMAS, ROCKET_GYROSCOPE_SIGMAS,
        ROTATING_GRAVITY,
    ),
    "Specified": nees.make_params(
        ROCKET_ACCELEROMETER_SIGMAS, ROCKET_GYROSCOPE_SIGMAS,
        ROTATING_GRAVITY, EARTH_RATE,
    ),
}

For $\theta=-\omega_E T$ , the independent truth calculation uses $A=\operatorname{Exp}(\theta)$ , $G^v=J_L(\theta)$ , and $G^p=J_L(\theta)-\Gamma_2(\theta)$ . Position and velocity blocks are translated from the paper to GTSAM’s (R,p,v) order.

ROTATING_TRUTH = nees.exact_rotating_state(
    STATE_I, ROTATING_ACCELERATION, ROTATING_ANGULAR_VELOCITY,
    ROTATING_DURATION, ROTATING_GRAVITY, EARTH_RATE,
)
nominal_rotating_accelerations = np.tile(
    ROTATING_ACCELERATION, (ROTATING_STEPS, 1)
)
nominal_rotating_omegas = np.tile(
    ROTATING_ANGULAR_VELOCITY, (ROTATING_STEPS, 1)
)
rotating_deterministic = nees.rotating_deterministic_samples(
    nominal_rotating_accelerations, nominal_rotating_omegas,
    ROTATING_DT, ROTATING_PARAMS, STATE_I, ROTATING_TRUTH,
)

print(
    f"Exact endpoint: altitude {ROTATING_TRUTH.position()[2]:.1f} m, "
    f"speed {np.linalg.norm(ROTATING_TRUTH.velocity()):.1f} m/s, "
    f"pitch {np.rad2deg(ROTATING_TRUTH.attitude().rpy()[1]):.1f} deg"
)
assert np.linalg.norm(rotating_deterministic["Galilean"]["Specified"]) < 1e-8
assert np.linalg.norm(rotating_deterministic["Galilean"]["Not specified"]) > 0.2

Exact endpoint: altitude 861.0 m, speed 431.1 m/s, pitch -9.2 deg

rotating = nees.run_rotating_nees(
    ROTATING_ACCELERATION, ROTATING_ANGULAR_VELOCITY,
    ROTATING_DURATION, ROTATING_DT, ROTATING_TRIALS, ROTATING_SEED,
    ROCKET_ACCELEROMETER_SIGMAS, ROCKET_GYROSCOPE_SIGMAS,
    ROTATING_PARAMS, STATE_I, ROTATING_TRUTH,
)
nees.validate_finite(rotating)
rotating_physical_table, rotating_nees_table = nees.rotating_tables(rotating)
display(Markdown(rotating_physical_table))
display(Markdown(rotating_nees_table))
print(nees.interval_text(rotating))

Expected 95% interval for mean NEES: [8.849, 9.152]

Supplying Earth rate improves every backend. The remaining gap between Galilean and the established methods is the held-input integration model, not the Logmap residual.

4. Long-horizon uncertainty with fixed nonzero bias¶

This ten-second experiment centers every backend on its own noise-free discrete mean. It therefore isolates accumulated sensor uncertainty while retaining a fixed, nonzero accelerometer and gyroscope bias.

UNCERTAINTY_DT = 0.1
UNCERTAINTY_STEPS = 100
UNCERTAINTY_TRIALS = 2_000
UNCERTAINTY_SEED = 2904
UNCERTAINTY_TIMES = (np.arange(UNCERTAINTY_STEPS) + 0.5) * UNCERTAINTY_DT
UNCERTAINTY_ACCELERATIONS = np.column_stack((
    0.8 + 0.35 * np.sin(0.37 * UNCERTAINTY_TIMES),
    -0.45 + 0.25 * np.cos(0.53 * UNCERTAINTY_TIMES),
    0.3 * np.sin(0.29 * UNCERTAINTY_TIMES),
))
UNCERTAINTY_OMEGAS = np.column_stack((
    0.35 * np.sin(0.41 * UNCERTAINTY_TIMES),
    -0.30 * np.cos(0.31 * UNCERTAINTY_TIMES),
    0.20 + 0.15 * np.sin(0.23 * UNCERTAINTY_TIMES),
))
UNCERTAINTY_ACCELEROMETER_SIGMAS = np.array([0.25, 0.30, 0.35])
UNCERTAINTY_GYROSCOPE_SIGMAS = np.array([0.07, 0.09, 0.11])
NONZERO_BIAS = gtsam.imuBias.ConstantBias(
    np.array([0.08, -0.05, 0.03]),
    np.array([0.004, -0.006, 0.005]),
)
UNCERTAINTY_PARAMS = nees.make_params(
    UNCERTAINTY_ACCELEROMETER_SIGMAS,
    UNCERTAINTY_GYROSCOPE_SIGMAS,
)

fixed_bias = nees.run_fixed_bias_nees(
    UNCERTAINTY_ACCELERATIONS, UNCERTAINTY_OMEGAS,
    UNCERTAINTY_DT, UNCERTAINTY_TRIALS, UNCERTAINTY_SEED,
    UNCERTAINTY_ACCELEROMETER_SIGMAS, UNCERTAINTY_GYROSCOPE_SIGMAS,
    UNCERTAINTY_PARAMS, NONZERO_BIAS, STATE_I,
)
nees.validate_finite(fixed_bias)
display(Markdown(nees.uncertainty_table(fixed_bias, 9)))
print(nees.interval_text(fixed_bias))

Expected 95% interval for mean NEES: [8.815, 9.187]

All four Logmap results should remain close to the theoretical mean. This experiment concerns the uncertainty representation; it does not yet perturb the optimizer’s bias away from the preintegration bias.

5. Full 15D uncertainty with bias random walk¶

BIAS_ACCELEROMETER_RW_SIGMAS = np.array([0.015, 0.020, 0.025])
BIAS_GYROSCOPE_RW_SIGMAS = np.array([0.002, 0.0025, 0.003])
BIAS_RW_SEED = 3904
COMBINED_PARAMS = nees.make_combined_params(
    UNCERTAINTY_ACCELEROMETER_SIGMAS,
    UNCERTAINTY_GYROSCOPE_SIGMAS,
    BIAS_ACCELEROMETER_RW_SIGMAS,
    BIAS_GYROSCOPE_RW_SIGMAS,
)

combined = nees.run_combined_bias_rw_nees(
    UNCERTAINTY_ACCELERATIONS, UNCERTAINTY_OMEGAS,
    UNCERTAINTY_DT, UNCERTAINTY_TRIALS, BIAS_RW_SEED,
    UNCERTAINTY_ACCELEROMETER_SIGMAS, UNCERTAINTY_GYROSCOPE_SIGMAS,
    BIAS_ACCELEROMETER_RW_SIGMAS, BIAS_GYROSCOPE_RW_SIGMAS,
    COMBINED_PARAMS, NONZERO_BIAS, STATE_I,
)
nees.validate_finite(combined)
display(Markdown(nees.uncertainty_table(combined, 15)))
print(nees.interval_text(combined))

Expected 95% interval for mean NEES: [14.761, 15.241]

The 15D residual retains the sampled bias path and the state-bias cross-covariance. The navigation block is always the $SE_2(3)$ Logmap; the final six rows are the bias random-walk residual.

6. Right-applied first-order bias correction¶

The paper defines the Galilean correction at a changed preintegration bias by one right update,

\widehat\Upsilon_{ij}(\hat b+\delta b) \simeq \widehat\Upsilon_{ij}(\hat b)\operatorname{Exp}(J_b\delta b).

(3)

This is the same perturbation side as the direct-product state’s standard left-invariant error, with the sensitivity recursion $J_{k+1}=\operatorname{Ad}_{V_k^{-1}}J_k-C_k$ . For each random update below, every backend’s first-order prediction is compared with complete reintegration of the same noise-free measurements. The linearization bias is nonzero, and $\lVert\delta b_a\rVert=30\lVert\delta b_\omega\rVert$ , following the bias-update scaling used by Brossard et al. Errors use only reintegratedState.logmap(correctedState).

BIAS_UPDATE_MAGNITUDES = np.array([
    0.0, 0.01, 0.02, 0.05, 0.10, 0.15, 0.20, 0.25, 0.30, 0.35
])
BIAS_UPDATE_TRIALS = 2_000
BIAS_UPDATE_SEED = 4904
BIAS_UPDATE_STEPS = round(1.0 / UNCERTAINTY_DT)

bias_update = nees.run_bias_update_sweep(
    UNCERTAINTY_ACCELERATIONS[:BIAS_UPDATE_STEPS],
    UNCERTAINTY_OMEGAS[:BIAS_UPDATE_STEPS],
    UNCERTAINTY_DT, UNCERTAINTY_PARAMS, NONZERO_BIAS, STATE_I,
    BIAS_UPDATE_MAGNITUDES, BIAS_UPDATE_TRIALS, BIAS_UPDATE_SEED,
)
nees.validate_bias_sweep(BIAS_UPDATE_MAGNITUDES, bias_update)
display(Markdown(nees.bias_update_table(BIAS_UPDATE_MAGNITUDES, bias_update)))

Manifold, Lie-group, and Galilean show nearly identical attitude-correction error here, while Tangent’s tangent-space update differs. The position and velocity curves expose whether a backend applies the complete group exponential correction or separately adds component derivatives. Near zero, all first-order approximation errors scale quadratically with bias-update magnitude.

7. Conclusions¶

Residual: use the default $SE_2(3)$ Logmap for IMU factors.
Bias correction: Galilean preintegration uses the companion paper’s direct-product right correction; Lie-group preintegration uses the analogous complete right-applied $SE_2(3)$ update in GTSAM’s (R,p,v) convention.
Rotating frames: supply omegaCoriolis; omitting known Earth rate creates deterministic error for every backend.
Backend: Galilean preintegration is most useful when simultaneous rotation and acceleration, lower rates, or longer held-input intervals make coupled integration accuracy important.

Legacy and ComponentWise remain API compatibility choices, but they are not active notebook experiment modes.