IMU factor covariance: NEES validation

This notebook validates the covariance used by GTSAM’s 9D IMU factor with a Monte Carlo Normalized Estimation Error Squared (NEES) experiment. It is adapted from the NEES notebook proposed in PR #2384, but focuses directly on the behavior changed by the covariance fix:

Old behavior: install the raw backend-native preintMeasCov().
Fixed behavior: install residualCovariance(), expressed in the factor’s existing residual chart.

The nonlinear residual and preintegration propagation are identical in both comparisons. Only the covariance used to score the residual changes.

import numpy as np
import plotly.graph_objects as go

import gtsam
from gtsam.symbol_shorthand import B, X

np.set_printoptions(precision=4, suppress=True)

Experiment¶

For each trial, we integrate 60 noisy IMU measurements and evaluate the actual ImuFactor2 residual at the states generated by the corresponding noiseless measurements. Sensor noise is sampled with the continuous-time convention used by preintegration: each measurement receives noise with standard deviation (\sigma/\sqrt{\Delta t}).

For a residual (e \in \mathbb{R}^9) and covariance (P),

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

A statistically consistent covariance has mean NEES near the residual dimension, 9. The old comparison uses preintMeasCov() to reproduce the previous factor noise model; the fixed comparison uses residualCovariance().

DT = 0.02
STEPS = 60
TRIALS = 5_000
SEED = 2384

ACCELERATION = np.array([0.3, -0.2, 9.7])
ANGULAR_VELOCITY = np.array([0.15, -0.10, 1.0])
ACCELEROMETER_SIGMAS = np.array([0.02, 0.08, 0.15])
GYROSCOPE_SIGMAS = np.array([0.01, 0.04, 0.08])


def make_params():
    params = gtsam.PreintegrationParams.MakeSharedD(10.0)
    params.setAccelerometerCovariance(np.diag(ACCELEROMETER_SIGMAS**2))
    params.setGyroscopeCovariance(np.diag(GYROSCOPE_SIGMAS**2))
    params.setIntegrationCovariance(np.zeros((3, 3)))
    return params

Baseline and fixed covariances¶

The ideal PIM provides both views of the same propagated uncertainty. We also extract the covariance actually installed in ImuFactor2 to verify that the factor uses the fixed view.

params = make_params()
bias = gtsam.imuBias.ConstantBias()

ideal_pim = gtsam.PreintegratedImuMeasurements(params, bias)
for _ in range(STEPS):
    ideal_pim.integrateMeasurement(
        ACCELERATION, ANGULAR_VELOCITY, DT
    )

state_i = gtsam.NavState()
state_j = ideal_pim.predict(state_i, bias)

old_covariance = np.asarray(ideal_pim.preintMeasCov())
fixed_covariance = np.asarray(ideal_pim.residualCovariance())
ideal_factor = gtsam.ImuFactor2(X(0), X(1), B(0), ideal_pim)
factor_covariance = np.asarray(ideal_factor.noiseModel().covariance())

np.testing.assert_allclose(
    factor_covariance, fixed_covariance, rtol=1e-10, atol=1e-12
)

Monte Carlo residuals¶

Every sample constructs the real factor and calls its nonlinear evaluateError; the experiment does not redefine or linearize the residual.

rng = np.random.default_rng(SEED)
residuals = np.empty((TRIALS, 9))

for trial in range(TRIALS):
    pim = gtsam.PreintegratedImuMeasurements(params, bias)
    accelerometer_noise = rng.normal(size=(STEPS, 3)) * (
        ACCELEROMETER_SIGMAS / np.sqrt(DT)
    )
    gyroscope_noise = rng.normal(size=(STEPS, 3)) * (
        GYROSCOPE_SIGMAS / np.sqrt(DT)
    )

    for step in range(STEPS):
        pim.integrateMeasurement(
            ACCELERATION + accelerometer_noise[step],
            ANGULAR_VELOCITY + gyroscope_noise[step],
            DT,
        )

    factor = gtsam.ImuFactor2(X(0), X(1), B(0), pim)
    residuals[trial] = factor.evaluateError(state_i, state_j, bias)

Statistical consistency¶

Besides mean NEES, relative Frobenius error compares each model covariance with the empirical covariance of the sampled factor residuals.

def nees_values(errors, covariance):
    solved = np.linalg.solve(covariance, errors.T).T
    return np.einsum("ni,ni->n", errors, solved)


def relative_frobenius(actual, expected):
    return np.linalg.norm(actual - expected, ord="fro") / np.linalg.norm(
        expected, ord="fro"
    )


empirical_covariance = np.cov(residuals, rowvar=False, ddof=1)
old_nees = nees_values(residuals, old_covariance)
fixed_nees = nees_values(residuals, fixed_covariance)

dimension = residuals.shape[1]
mean_nees_standard_error = np.sqrt(2.0 * dimension / TRIALS)
mean_nees_95 = (
    dimension - 1.96 * mean_nees_standard_error,
    dimension + 1.96 * mean_nees_standard_error,
)

summary = {
    "expected_mean_nees": float(dimension),
    "mean_nees_95_lower": mean_nees_95[0],
    "mean_nees_95_upper": mean_nees_95[1],
    "old_mean_nees": float(old_nees.mean()),
    "fixed_mean_nees": float(fixed_nees.mean()),
    "old_relative_covariance_error": relative_frobenius(
        old_covariance, empirical_covariance
    ),
    "fixed_relative_covariance_error": relative_frobenius(
        fixed_covariance, empirical_covariance
    ),
    "factor_vs_fixed_relative_error": relative_frobenius(
        factor_covariance, fixed_covariance
    ),
}

for name, value in summary.items():
    print(f"{name:>36}: {value:.6f}")

# These assertions turn the notebook into an executable validation.
assert not mean_nees_95[0] <= summary["old_mean_nees"] <= mean_nees_95[1]
assert mean_nees_95[0] <= summary["fixed_mean_nees"] <= mean_nees_95[1]
assert summary["fixed_relative_covariance_error"] < 0.05
assert (
    summary["fixed_relative_covariance_error"]
    < summary["old_relative_covariance_error"]
)
np.testing.assert_allclose(
    factor_covariance, fixed_covariance, rtol=1e-10, atol=1e-12
)

nees_deviation_reduction = 1.0 - (
    abs(summary["fixed_mean_nees"] - dimension)
    / abs(summary["old_mean_nees"] - dimension)
)
covariance_error_reduction = 1.0 - (
    summary["fixed_relative_covariance_error"]
    / summary["old_relative_covariance_error"]
)

print(f"NEES deviation reduction: {nees_deviation_reduction:.2%}")
print(f"Covariance error reduction: {covariance_error_reduction:.2%}")

Visual comparison¶

fig = go.Figure()
fig.add_bar(
    x=["Old: raw covariance", "Fixed: residual covariance"],
    y=[summary["old_mean_nees"], summary["fixed_mean_nees"]],
    marker_color=["#d95f02", "#1b9e77"],
)
fig.add_hline(
    y=dimension,
    line_dash="dash",
    line_color="black",
    annotation_text="Expected mean NEES = 9",
)
fig.add_hrect(
    y0=mean_nees_95[0],
    y1=mean_nees_95[1],
    fillcolor="#1b9e77",
    opacity=0.15,
    line_width=0,
    annotation_text="Approximate 95% interval",
)
fig.update_layout(
    title="IMU factor residual consistency",
    xaxis_title="Covariance supplied to the factor residual",
    yaxis_title="Mean NEES",
    showlegend=False,
)
fig.show()

fig = go.Figure()
fig.add_bar(
    x=["Old: raw covariance", "Fixed: residual covariance"],
    y=[
        summary["old_relative_covariance_error"],
        summary["fixed_relative_covariance_error"],
    ],
    marker_color=["#d95f02", "#1b9e77"],
)
fig.update_layout(
    title="Model covariance versus empirical residual covariance",
    xaxis_title="Covariance model",
    yaxis_title="Relative Frobenius error",
    showlegend=False,
)
fig.show()

Interpretation¶

With the deterministic seed above, the previous raw covariance gives mean NEES about 23.7, far outside the expected range around 9. The corrected residual-chart covariance gives mean NEES about 9.1, inside that range.

The relative mismatch with the empirical factor-residual covariance falls from about 97.5% to 2.75%. The installed factor covariance and residualCovariance() agree to numerical precision.

This demonstrates a statistical-consistency improvement in the IMU factor’s noise model. It does not claim a runtime improvement, and it deliberately does not change the nonlinear residual or the raw propagated covariance.