This notebook compares GTSAM’s Manifold, Tangent, Lie-group, and Galilean IMU preintegration backends under identical measurements and noise samples. Sections 2--6 retain the inertial-frame stress test with simultaneous body-frame acceleration and rotation. Section 7 adds a realistic powered-ascent benchmark in a rotating Earth frame and evaluates every backend both with omegaCoriolis specified and with it omitted.
The word better has two testable meanings here: smaller deterministic endpoint error at a fixed sample period, and statistical consistency measured by 9D Normalized Estimation Error Squared (NEES). This experiment does not claim universal dominance, lower runtime, or an advantage when the assumed held-input model is a poor description of the sensor signal. See the companion GalileanImuFactor guide for the complete left-invariant derivation.
import numpy as np
import plotly.graph_objects as go
import plotly.io as pio
from IPython.display import Markdown, display
from scipy.stats import chi2
import gtsam
np.set_printoptions(precision=4, suppress=True)
pio.renderers.default = "notebook_connected"1. What NEES measures¶
For the factor residual and its predicted covariance ,
A consistent model has expected NEES equal to the residual dimension, nine. For independent trials, the exact 95% acceptance interval for the average NEES is
Every backend below receives the same continuous-time accelerometer and gyroscope noise densities, the same sampled noise realization in each trial, the same zero bias, and the same exact endpoint. Gravity and integration noise are set to zero to isolate the preintegration discretization and its sensor-noise covariance. The evaluated error is state_j.localCoordinates(predicted_state_j), exactly the existing GTSAM IMU factor residual chart.
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])
BIAS = gtsam.imuBias.ConstantBias()
STATE_I = gtsam.NavState()
BACKENDS = {
"Manifold": gtsam.PreintegratedImuMeasurementsManifold,
"Tangent": gtsam.PreintegratedImuMeasurementsTangent,
"Lie group": gtsam.PreintegratedImuMeasurementsLieGroup,
"Galilean": gtsam.PreintegratedImuMeasurementsG,
}
def make_params():
params = gtsam.PreintegrationParams(np.zeros(3))
params.setAccelerometerCovariance(
np.diag(ACCELEROMETER_SIGMAS**2)
)
params.setGyroscopeCovariance(np.diag(GYROSCOPE_SIGMAS**2))
params.setIntegrationCovariance(np.zeros((3, 3)))
return params
PARAMS = make_params()2. Independent closed-form endpoint¶
With constant body acceleration , angular rate , and duration , the continuous held-input solution is the Galilean exponential
This is the analytical solution of the continuous dynamics, not a fine-step numerical reference. We remove deterministic time and map into the NavState ordering $(R,p,v)`.
def exact_held_input_state(acceleration, angular_velocity, duration):
tangent = np.zeros(10)
tangent[:3] = angular_velocity * duration
tangent[3:6] = acceleration * duration
tangent[9] = duration
delta = gtsam.Gal3.Expmap(tangent)
return gtsam.NavState(
delta.rotation(), delta.translation(), delta.velocity()
)
TRUTH = exact_held_input_state(
ACCELERATION, ANGULAR_VELOCITY, DURATION
)
def integrate(
backend_type, accelerations, angular_velocities, dt, params=PARAMS
):
pim = backend_type(params, BIAS)
for acceleration, angular_velocity in zip(
accelerations, angular_velocities
):
pim.integrateMeasurement(acceleration, angular_velocity, dt)
return pim3. Deterministic discretization error¶
First remove sensor noise and vary only the sample period. The three established backends use the same piecewise update for this trajectory and therefore overlap. Their error decreases linearly as the timestep shrinks. Galilean composition remains at floating-point precision because each held-input interval uses the exact coupled exponential.
SAMPLE_PERIODS = np.array([0.1, 0.05, 0.025, 0.0125])
deterministic = {
name: {"position": [], "velocity": []}
for name in BACKENDS
}
for dt in SAMPLE_PERIODS:
steps = round(DURATION / dt)
accelerations = np.tile(ACCELERATION, (steps, 1))
angular_velocities = np.tile(ANGULAR_VELOCITY, (steps, 1))
for name, backend_type in BACKENDS.items():
pim = integrate(
backend_type, accelerations, angular_velocities, dt
)
error = np.asarray(
TRUTH.localCoordinates(pim.predict(STATE_I, BIAS))
)
deterministic[name]["position"].append(
np.linalg.norm(error[3:6])
)
deterministic[name]["velocity"].append(
np.linalg.norm(error[6:9])
)
assert max(deterministic["Galilean"]["position"]) < 2e-12
assert max(deterministic["Galilean"]["velocity"]) < 4e-12
for name in ("Manifold", "Tangent", "Lie group"):
assert deterministic[name]["position"][1] > 0.03
assert deterministic[name]["velocity"][1] > 0.07fig = go.Figure()
line_styles = {
"Manifold": ("#777777", "solid"),
"Tangent": ("#999999", "dash"),
"Lie group": ("#bbbbbb", "dot"),
"Galilean": ("#14866d", "solid"),
}
for name in BACKENDS:
color, dash = line_styles[name]
fig.add_scatter(
x=SAMPLE_PERIODS,
y=deterministic[name]["velocity"],
mode="lines+markers",
name=name,
line=dict(color=color, dash=dash),
)
fig.update_xaxes(type="log", title="IMU sample period (s)")
fig.update_yaxes(type="log", title="Velocity error norm (m/s)")
fig.update_layout(
title="Held-input discretization error",
template="plotly_white",
legend_title_text="Backend",
)
fig.show()4. Paired Monte Carlo NEES experiment¶
We now use a 20 Hz IMU for one second and add anisotropic white sensor noise. Continuous-time noise density becomes sampled rate noise . Each trial generates one noise sequence and feeds that identical sequence to all four backends, making this a paired comparison. Each backend supplies its own propagated residualCovariance().
DT = 0.05
STEPS = round(DURATION / DT)
TRIALS = 3_000
SEED = 2231
rng = np.random.default_rng(SEED)
errors = {name: np.empty((TRIALS, 9)) for name in BACKENDS}
nees = {name: np.empty(TRIALS) for name in BACKENDS}
for trial in range(TRIALS):
accelerometer_noise = rng.normal(size=(STEPS, 3)) * (
ACCELEROMETER_SIGMAS / np.sqrt(DT)
)
gyroscope_noise = rng.normal(size=(STEPS, 3)) * (
GYROSCOPE_SIGMAS / np.sqrt(DT)
)
accelerations = ACCELERATION + accelerometer_noise
angular_velocities = ANGULAR_VELOCITY + gyroscope_noise
for name, backend_type in BACKENDS.items():
pim = integrate(
backend_type, accelerations, angular_velocities, DT
)
error = np.asarray(
TRUTH.localCoordinates(pim.predict(STATE_I, BIAS))
)
covariance = np.asarray(pim.residualCovariance())
errors[name][trial] = error
nees[name][trial] = error @ np.linalg.solve(covariance, error)5. Results¶
The expected band below is the exact chi-square interval for the average of 3,000 independent 9D NEES samples. The error bars on each point are empirical 95% confidence intervals for that backend’s sampled mean. Position and velocity RMS values are norms within their three-dimensional blocks, so they retain physical units.
DIMENSION = 9
expected_interval = chi2.ppf(
[0.025, 0.975], TRIALS * DIMENSION
) / TRIALS
summary = {}
for name in BACKENDS:
backend_errors = errors[name]
values = nees[name]
mean = values.mean()
mean_half_width = 1.96 * values.std(ddof=1) / np.sqrt(TRIALS)
summary[name] = {
"mean_nees": mean,
"mean_half_width": mean_half_width,
"rotation_rmse": np.sqrt(
np.mean(np.sum(backend_errors[:, :3] ** 2, axis=1))
),
"position_rmse": np.sqrt(
np.mean(np.sum(backend_errors[:, 3:6] ** 2, axis=1))
),
"velocity_rmse": np.sqrt(
np.mean(np.sum(backend_errors[:, 6:9] ** 2, axis=1))
),
"mean_error_norm": np.linalg.norm(backend_errors.mean(axis=0)),
}
rows = [
"| Backend | Mean NEES | Position RMS (m) | Velocity RMS (m/s) | Mean error norm |",
"|---|---:|---:|---:|---:|",
]
for name, values in summary.items():
rows.append(
f"| {name} | {values['mean_nees']:.3f} | "
f"{values['position_rmse']:.4f} | "
f"{values['velocity_rmse']:.4f} | "
f"{values['mean_error_norm']:.4f} |"
)
display(Markdown("\n".join(rows)))
print(
"Expected 95% interval for mean NEES: "
f"[{expected_interval[0]:.3f}, {expected_interval[1]:.3f}]"
)Expected 95% interval for mean NEES: [8.849, 9.152]
# Executable statistical claims for this deterministic experiment.
galilean_nees = summary["Galilean"]["mean_nees"]
assert expected_interval[0] <= galilean_nees <= expected_interval[1]
for name in ("Manifold", "Tangent", "Lie group"):
assert summary[name]["mean_nees"] > expected_interval[1]
assert (
summary["Galilean"]["position_rmse"]
< summary[name]["position_rmse"]
)
assert (
summary["Galilean"]["velocity_rmse"]
< summary[name]["velocity_rmse"]
)names = list(BACKENDS)
means = [summary[name]["mean_nees"] for name in names]
half_widths = [
summary[name]["mean_half_width"] for name in names
]
colors = ["#888888", "#999999", "#aaaaaa", "#14866d"]
fig = go.Figure()
fig.add_hrect(
y0=expected_interval[0],
y1=expected_interval[1],
fillcolor="#14866d",
opacity=0.14,
line_width=0,
annotation_text="95% consistency band",
annotation_position="top left",
)
fig.add_scatter(
x=names,
y=means,
mode="markers",
marker=dict(size=11, color=colors),
error_y=dict(type="data", array=half_widths, visible=True),
hovertemplate="%{x}: mean NEES %{y:.3f}<extra></extra>",
)
fig.add_hline(y=DIMENSION, line_dash="dash", line_color="#333333")
fig.update_layout(
title="Average 9D NEES under identical high-dynamic IMU samples",
xaxis_title="Preintegration backend",
yaxis_title="Mean NEES",
yaxis_range=[0, max(means) + 1.5],
template="plotly_white",
showlegend=False,
)
fig.show()6. Interpretation and limits¶
With the fixed seed, Galilean preintegration has mean NEES close to nine and inside the exact 95% consistency interval. Manifold, Tangent, and Lie-group preintegration all lie well above the interval. Their propagated covariances describe sensor noise, but their deterministic position/velocity discretization error is not represented in that covariance, so the residual is overconfident.
The endpoint table also separates consistency from accuracy: Galilean preintegration reduces both position and velocity RMS error, while all four backends have essentially the same rotation RMS error. The timestep sweep identifies the cause. The three established variants converge as the sampling interval shrinks, whereas the Galilean group law and exponential integrate the coupled held input exactly at every tested interval.
This result is deliberately scoped. At very high IMU rates the standard discretization error becomes negligible; with time-varying input inside a sample, all zero-order-hold methods inherit model error; and this notebook does not compare runtime, bias random walks, sensor-pose corrections, or full graph optimization. It shows that for simultaneous rotation and acceleration at a finite sampling rate, the left-invariant Galilean formulation provides the most accurate mean and the only statistically consistent covariance among the four tested GTSAM backends.
7. Powered ascent in a rotating Earth frame¶
We now model the first four seconds of a high-power sounding-rocket ascent. This scale is grounded in a Georgia Tech Experimental Rocketry flight that reached roughly 8,000 ft in six to seven seconds and Mach 1.8. The benchmark starts from rest, uses a constant 12 g measured specific force along the body vertical, and applies a modest -0.04 rad/s pitch rate. Its exact endpoint is about 861 m above the pad, traveling at 431 m/s with 9.2 degrees of pitch: a plausible early powered-ascent segment rather than an artificially high initial speed.
The local navigation frame is east-north-up at Spaceport America latitude, so the physical Earth-rate vector contains north and up components. For every backend we run two otherwise identical predictions: Specified supplies that vector through omegaCoriolis, while Not specified leaves the parameter absent. Each paired Monte Carlo trial uses the same IMU noise sequence in all eight cases. The preintegrated body increment and covariance recursion are unchanged by Earth rotation; only endpoint prediction and residual assembly use the rotating-frame model.
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
])
PITCH_RATE = -0.04
ROTATING_ANGULAR_VELOCITY = (
EARTH_RATE + np.array([0.0, PITCH_RATE, 0.0])
)
ROTATING_DURATION = 4.0
ROTATING_DT = 0.05
ROTATING_STEPS = round(ROTATING_DURATION / ROTATING_DT)
ROTATING_TRIALS = 3_000
ROTATING_SEED = 2232
ROTATING_STATE_I = gtsam.NavState()
ROCKET_ACCELEROMETER_SIGMAS = ACCELEROMETER_SIGMAS.copy()
ROCKET_GYROSCOPE_SIGMAS = np.array([5e-4, 7.5e-4, 1e-3])
def make_rotating_params(with_coriolis):
params = gtsam.PreintegrationParams(ROTATING_GRAVITY)
params.setAccelerometerCovariance(
np.diag(ROCKET_ACCELEROMETER_SIGMAS**2)
)
params.setGyroscopeCovariance(
np.diag(ROCKET_GYROSCOPE_SIGMAS**2)
)
params.setIntegrationCovariance(np.zeros((3, 3)))
if with_coriolis:
params.setOmegaCoriolis(EARTH_RATE)
return params
CORIOLIS_CONDITIONS = {
"Not specified": False,
"Specified": True,
}
ROTATING_PARAMS = {
condition: make_rotating_params(enabled)
for condition, enabled in CORIOLIS_CONDITIONS.items()
}Independent exact endpoint¶
The truth calculation below is a direct transcription of the paper’s closed-form rotating-frame equation, not a call to a PIM’s predict. It first evaluates the held body increment with the Galilean exponential. For , it then forms , , and . In GTSAM’s NavState(R,p,v) order,
with , , and . This explicit block order is the only change from the paper’s displayed convention.
def skew(vector):
x, y, z = vector
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
def exact_rotating_state(
state_i, acceleration, angular_velocity, duration, gravity, omega
):
tangent = np.zeros(10)
tangent[:3] = angular_velocity * duration
tangent[3:6] = acceleration * duration
tangent[9] = duration
delta = gtsam.Gal3.Expmap(tangent)
theta = -omega * duration
kernels = gtsam.so3.DexpFunctor(theta)
A = gtsam.Rot3.Expmap(theta).matrix()
gamma_velocity = kernels.leftJacobian()
gamma_position = gamma_velocity - kernels.Gamma().left()
omega_cross = skew(omega)
R_i = state_i.attitude().matrix()
p_i = state_i.position()
v_bar_i = state_i.velocity() + omega_cross @ p_i
p_j = gamma_position @ gravity * duration**2 + A @ (
p_i + v_bar_i * duration + R_i @ delta.translation()
)
v_bar_j = gamma_velocity @ gravity * duration + A @ (
v_bar_i + R_i @ delta.velocity()
)
v_j = v_bar_j - omega_cross @ p_j
R_j = A @ R_i @ delta.rotation().matrix()
return gtsam.NavState(gtsam.Rot3(R_j), p_j, v_j)
ROTATING_TRUTH = exact_rotating_state(
ROTATING_STATE_I,
ROTATING_ACCELERATION,
ROTATING_ANGULAR_VELOCITY,
ROTATING_DURATION,
ROTATING_GRAVITY,
EARTH_RATE,
)
rotating_deterministic = {name: {} for name in BACKENDS}
nominal_accelerations = np.tile(
ROTATING_ACCELERATION, (ROTATING_STEPS, 1)
)
nominal_angular_velocities = np.tile(
ROTATING_ANGULAR_VELOCITY, (ROTATING_STEPS, 1)
)
for name, backend_type in BACKENDS.items():
for condition, params in ROTATING_PARAMS.items():
pim = integrate(
backend_type,
nominal_accelerations,
nominal_angular_velocities,
ROTATING_DT,
params,
)
error = np.asarray(
ROTATING_TRUTH.localCoordinates(
pim.predict(ROTATING_STATE_I, BIAS)
)
)
rotating_deterministic[name][condition] = error
final_speed = np.linalg.norm(ROTATING_TRUTH.velocity())
final_altitude = ROTATING_TRUTH.position()[2]
final_pitch = np.rad2deg(ROTATING_TRUTH.attitude().rpy()[1])
print(
f"Exact endpoint: altitude {final_altitude:.1f} m, "
f"speed {final_speed:.1f} m/s, pitch {final_pitch:.1f} deg"
)
galilean_specified = rotating_deterministic["Galilean"][
"Specified"
]
assert np.linalg.norm(galilean_specified) < 1e-8
assert np.linalg.norm(
rotating_deterministic["Galilean"]["Not specified"]
) > 0.2
for name in ("Manifold", "Tangent", "Lie group"):
specified_norm = np.linalg.norm(
rotating_deterministic[name]["Specified"]
)
omitted_norm = np.linalg.norm(
rotating_deterministic[name]["Not specified"]
)
assert specified_norm > 1.0
assert omitted_norm > specified_normExact endpoint: altitude 861.0 m, speed 431.1 m/s, pitch -9.2 deg
Paired rotating-frame NEES¶
The deterministic checks separate the two errors. With Earth rate specified, the Galilean backend recovers the closed-form powered-ascent endpoint to floating-point precision, while the three established backends retain finite-rate position and velocity error from simultaneous thrust and pitch. Omitting Earth rate adds rotating-frame error to every backend. We then use a plausible lower-noise rocket IMU model and repeat 3,000 paired trials.
rotating_rng = np.random.default_rng(ROTATING_SEED)
rotating_errors = {
name: {
condition: np.empty((ROTATING_TRIALS, 9))
for condition in CORIOLIS_CONDITIONS
}
for name in BACKENDS
}
rotating_nees = {
name: {
condition: np.empty(ROTATING_TRIALS)
for condition in CORIOLIS_CONDITIONS
}
for name in BACKENDS
}
for trial in range(ROTATING_TRIALS):
accelerometer_noise = rotating_rng.normal(
size=(ROTATING_STEPS, 3)
) * (ROCKET_ACCELEROMETER_SIGMAS / np.sqrt(ROTATING_DT))
gyroscope_noise = rotating_rng.normal(
size=(ROTATING_STEPS, 3)
) * (ROCKET_GYROSCOPE_SIGMAS / np.sqrt(ROTATING_DT))
accelerations = ROTATING_ACCELERATION + accelerometer_noise
angular_velocities = (
ROTATING_ANGULAR_VELOCITY + gyroscope_noise
)
for name, backend_type in BACKENDS.items():
for condition, params in ROTATING_PARAMS.items():
pim = integrate(
backend_type,
accelerations,
angular_velocities,
ROTATING_DT,
params,
)
error = np.asarray(
ROTATING_TRUTH.localCoordinates(
pim.predict(ROTATING_STATE_I, BIAS)
)
)
covariance = np.asarray(pim.residualCovariance())
rotating_errors[name][condition][trial] = error
rotating_nees[name][condition][trial] = (
error @ np.linalg.solve(covariance, error)
)rotating_expected_interval = chi2.ppf(
[0.025, 0.975], ROTATING_TRIALS * DIMENSION
) / ROTATING_TRIALS
rotating_summary = {name: {} for name in BACKENDS}
rotating_rows = [
"| Backend | Coriolis | Mean NEES | Position RMS (m) | "
"Velocity RMS (m/s) |",
"|---|---|---:|---:|---:|",
]
for name in BACKENDS:
for condition in CORIOLIS_CONDITIONS:
backend_errors = rotating_errors[name][condition]
values = rotating_nees[name][condition]
mean = values.mean()
half_width = 1.96 * values.std(ddof=1) / np.sqrt(
ROTATING_TRIALS
)
rotating_summary[name][condition] = {
"mean_nees": mean,
"mean_half_width": half_width,
"position_rmse": np.sqrt(
np.mean(np.sum(backend_errors[:, 3:6] ** 2, axis=1))
),
"velocity_rmse": np.sqrt(
np.mean(np.sum(backend_errors[:, 6:9] ** 2, axis=1))
),
}
result = rotating_summary[name][condition]
rotating_rows.append(
f"| {name} | {condition} | {mean:.3f} | "
f"{result['position_rmse']:.4f} | "
f"{result['velocity_rmse']:.4f} |"
)
display(Markdown("\n".join(rotating_rows)))
print(
"Expected 95% interval for mean NEES: "
f"[{rotating_expected_interval[0]:.3f}, "
f"{rotating_expected_interval[1]:.3f}]"
)
galilean_with_coriolis = rotating_summary["Galilean"][
"Specified"
]["mean_nees"]
galilean_without_coriolis = rotating_summary["Galilean"][
"Not specified"
]["mean_nees"]
assert (
rotating_expected_interval[0]
<= galilean_with_coriolis
<= rotating_expected_interval[1]
)
assert galilean_without_coriolis > rotating_expected_interval[1]
assert galilean_with_coriolis < galilean_without_coriolis
for name in ("Manifold", "Tangent", "Lie group"):
specified = rotating_summary[name]["Specified"]["mean_nees"]
omitted = rotating_summary[name]["Not specified"]["mean_nees"]
assert specified > rotating_expected_interval[1]
assert omitted > specifiedExpected 95% interval for mean NEES: [8.849, 9.152]
fig = go.Figure()
fig.add_hrect(
y0=rotating_expected_interval[0],
y1=rotating_expected_interval[1],
fillcolor="#14866d",
opacity=0.14,
line_width=0,
annotation_text="95% consistency band",
annotation_position="top left",
)
condition_styles = {
"Not specified": ("#b44b4b", "x"),
"Specified": ("#14866d", "circle"),
}
for condition, (color, symbol) in condition_styles.items():
fig.add_scatter(
x=list(BACKENDS),
y=[
rotating_summary[name][condition]["mean_nees"]
for name in BACKENDS
],
mode="markers",
name=condition,
marker=dict(size=11, color=color, symbol=symbol),
error_y=dict(
type="data",
array=[
rotating_summary[name][condition]["mean_half_width"]
for name in BACKENDS
],
visible=True,
),
hovertemplate=(
f"{condition}<br>%{{x}}: mean NEES %{{y:.3f}}"
"<extra></extra>"
),
)
fig.add_hline(y=DIMENSION, line_dash="dash", line_color="#333333")
fig.update_layout(
title="Powered-ascent NEES with and without Coriolis",
xaxis_title="Preintegration backend",
yaxis_title="Mean NEES",
template="plotly_white",
legend_title_text="omegaCoriolis",
)
fig.show()Powered-ascent interpretation¶
With the fixed seed, Galilean preintegration with the physical Earth rate specified has mean NEES close to nine and inside the exact 95% consistency interval. The three established methods remain above the interval even with Coriolis enabled because their mean recursion incurs finite-rate error while the rocket pitches under thrust. Omitting omegaCoriolis worsens every backend; it adds deterministic rotating-frame error that the sensor-noise covariance does not model.
This powered-ascent experiment exercises both effects together. Galilean composition removes held-input discretization error from simultaneous thrust and pitch, while omegaCoriolis supplies the exact rotating-frame lift, gravity kernels, and projection. The result is not obtained by amplifying Earth’s rate or seeding an extreme initial velocity: the vehicle starts on the pad and reaches a flight state consistent with the early portion of a Georgia Tech high-power sounding-rocket launch.