ImuFactor - GTSAM Docs

Overview¶

ImuFactor (5-way Factor)¶

The ImuFactor is the standard GTSAM factor for incorporating preintegrated IMU measurements into a factor graph. It’s a 5-way factor connecting:

Pose at time $i$ (Pose3)
Velocity at time $i$ (Vector3)
Pose at time $j$ (Pose3)
Velocity at time $j$ (Vector3)
IMU Bias at time $i$ (imuBias::ConstantBias)

It takes a PreintegratedImuMeasurements object, which summarizes the IMU readings between times $i$ and $j$ . The factor’s error function measures the discrepancy between the relative motion predicted by the preintegrated measurements (corrected for the current estimate of the bias at time $i$ ) and the relative motion implied by the state variables ( $Pose_i, Vel_i, Pose_j, Vel_j$ ) connected to the factor.

ImuFactor2: NavState Variant¶

The ImuFactor2 is ternary variant of the ImuFactor that operates directly on NavState objects instead of separate Pose3 and Vector3 variables for pose and velocity. This simplifies the factor graph by reducing the number of connected variables and can make the graph more efficient to optimize.

Instead of connecting five variables (Pose_i, Vel_i, Pose_j, Vel_j, Bias_i), the ImuFactor2 connects three:

NavState at time $i$ (NavState combines pose and velocity)
NavState at time $j`
IMU Bias at time $i$ (imuBias::ConstantBias)

Modeling Bias¶

Both factors assume that the bias is constant between time $i$ and $j$ for the purpose of evaluating its error. That is typically a very good assumption, as bias evolves slowly over time.

The factors do not model the evolution of bias over time; if bias is expected to change, separate BetweenFactors on bias variables are typically needed, or the CombinedImuFactor should be used instead.

%pip install --quiet gtsam-develop

ERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)
ERROR: No matching distribution found for gtsam-develop
Note: you may need to restart the kernel to use updated packages.

Mathematical Formulation¶

Let $X_i = (R_i, p_i, v_i)$ be the state (Attitude, Position, Velocity) at time $i$ , and $b_i$ be the bias estimate at time $i$ . Let $X_j = (R_j, p_j, v_j)$ be the state at time $j$ .

The PreintegratedImuMeasurements object (pim) provides:

$\Delta \tilde{R}_{ij}(b_{est})$ , $\Delta \tilde{p}_{ij}(b_{est})$ , $\Delta \tilde{v}_{ij}(b_{est})$ : Preintegrated measurements calculated using the fixed bias estimate $b_{est}$ (pim.biasHat()).
Jacobians of the Δ terms with respect to the bias.

The factor first uses the pim object’s predict method (or equivalent calculations) to find the predicted state $X_{j,pred}$ based on $X_i$ and the current estimate of the bias $b_i$ from the factor graph values:

X_{j,pred} = \text{pim.predict}(X_i, b_i)

(1)

This prediction internally applies first-order corrections to the stored Δ values based on the difference between $b_i$ and $b_{est}$ .

The factor’s 9-dimensional error vector $e$ is then calculated as the difference between the predicted state $X_{j,pred}$ and the actual state $X_j$ in the tangent space of $X_{j,pred}$ :

e = \text{Logmap}_{X_{j,pred}}(X_j) = X_{j,pred}^{-1} \otimes X_j \quad \text{(Conceptual Lie notation)}

(2)

Or, more explicitly using the NavState::Logmap definition:

e = \text{NavState::Logmap}(X_{j,pred}.inverse() * X_j)

(3)

This error vector has components corresponding to errors in rotation (3), position (3), and velocity (3).

Key Functionality / API¶

Constructor: ImuFactor(keyPose_i, keyVel_i, keyPose_j, keyVel_j, keyBias_i, pim): Creates the factor, linking the five state/bias keys and providing the preintegrated measurements.
preintegratedMeasurements(): Returns a const reference to the stored PreintegratedImuMeasurements object.
evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i): Calculates the 9-dimensional error vector given the current values of the connected variables. Also computes Jacobians if requested.
noiseModel(): Returns the noise model associated with the preintegrated measurement uncertainty (derived from pim.preintMeasCov()).
print / equals: Standard factor methods.

Usage Example¶

Create parameters, a PIM object, define keys, and construct the factor.

from gtsam import PreintegrationParams, PreintegratedImuMeasurements, ImuFactor
from gtsam import NonlinearFactorGraph, Values, NavState, Pose3
from gtsam.symbol_shorthand import X,V,B
from gtsam.imuBias import ConstantBias
import numpy as np

# 1. Create Parameters and PIM (as in PreintegratedImuMeasurements example)
params = PreintegrationParams.MakeSharedU(9.81)
accel_noise_sigma = 0.1
gyro_noise_sigma = 0.01
params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)
params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)
params.setIntegrationCovariance(np.eye(3) * 1e-8)
bias_hat = ConstantBias() # Assume zero bias used for preintegration
pim = PreintegratedImuMeasurements(params, bias_hat)

# Integrate some dummy measurements
dt = 0.01
acc_meas = np.array([0.0, 0.0, -9.81]) # Stationary
gyro_meas = np.array([0.0, 0.0, 0.0]) # Stationary
for _ in range(10):
    pim.integrateMeasurement(acc_meas, gyro_meas, dt)

# 2. Symbolic Keys will be X(0), V(0), X(1), V(1), B(0)

# 3. Create the ImuFactor
# The noise model is automatically derived from pim.preintMeasCov()
imu_factor = ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)

print("Created ImuFactor:")
imu_factor.print()

# 4. Example: Evaluate error with perfect states (should be near zero)
graph = NonlinearFactorGraph()
graph.add(imu_factor)

values = Values()
pose_i = Pose3() # Identity
vel_i = np.zeros(3)
bias_i = ConstantBias() # Zero bias

# Predict state j using the PIM and the *same* bias used for integration
nav_state_i = NavState(pose_i, vel_i)
nav_state_j = pim.predict(nav_state_i, bias_i) # Use bias_i=bias_hat
pose_j = nav_state_j.pose()
vel_j = nav_state_j.velocity()

values.insert(X(0), pose_i)
values.insert(V(0), vel_i)
values.insert(X(1), pose_j)
values.insert(V(1), vel_j)
values.insert(B(0), bias_i)

error_vector = imu_factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i)
print("\nError vector (should be near zero):", error_vector)
print("Factor error (0.5 * ||error||^2_Sigma):", graph.error(values))

Created ImuFactor:
ImuFactor(x0,v0,x1,v1,b0)
preintegrated measurements:

    deltaTij = 0.1
    deltaRij.ypr = ( 0 -0  0)
    deltaPij =        0        0 -0.04905
    deltaVij =      0      0 -0.981
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[       1e-05            0            0            0  1.39793e-07            0            0   4.4145e-06            0
           0        1e-05            0 -1.39793e-07            0            0  -4.4145e-06            0            0
           0            0        1e-05            0            0            0            0            0            0
           0 -1.39793e-07            0  3.32969e-06            0            0  5.00974e-05            0            0
 1.39793e-07            0            0            0  3.32969e-06            0            0  5.00974e-05            0
           0            0            0            0            0    3.326e-06            0            0        5e-05
           0  -4.4145e-06            0  5.00974e-05            0            0   0.00100274            0            0
  4.4145e-06            0            0            0  5.00974e-05            0            0   0.00100274            0
           0            0            0            0            0        5e-05            0            0        0.001]
  noise model sigmas: 0.00316228 0.00316228 0.00316228 0.00182474 0.00182474 0.00182373  0.0316661  0.0316661  0.0316228

Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]
Factor error (0.5 * ||error||^2_Sigma): 0.0

We can also use ImuFactor2, with NavState, giving exactly the same result:

from gtsam import ImuFactor2

# 1. Create the ImuFactor2
# The noise model is automatically derived from pim.preintMeasCov()
imu_factor2 = ImuFactor2(X(0), X(1), B(0), pim)

print("Created ImuFactor2:")
imu_factor2.print()

# 2. Example: Evaluate error with perfect states (should be near zero)
graph = NonlinearFactorGraph()
graph.add(imu_factor2)

values = Values()
nav_state_i = NavState(pose_i, vel_i)
nav_state_j = pim.predict(nav_state_i, bias_i) # Use bias_i=bias_hat

values.insert(X(0), nav_state_i)
values.insert(X(1), nav_state_j)
values.insert(B(0), bias_i)

error_vector = imu_factor2.evaluateError(nav_state_i, nav_state_j, bias_i)
print("\nError vector (should be near zero):", error_vector)
print("Factor error (0.5 * ||error||^2_Sigma):", graph.error(values))

Created ImuFactor2:
ImuFactor2(x0,x1,b0)
preintegrated measurements:

    deltaTij = 0.1
    deltaRij.ypr = ( 0 -0  0)
    deltaPij =        0        0 -0.04905
    deltaVij =      0      0 -0.981
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[       1e-05            0            0            0  1.39793e-07            0            0   4.4145e-06            0
           0        1e-05            0 -1.39793e-07            0            0  -4.4145e-06            0            0
           0            0        1e-05            0            0            0            0            0            0
           0 -1.39793e-07            0  3.32969e-06            0            0  5.00974e-05            0            0
 1.39793e-07            0            0            0  3.32969e-06            0            0  5.00974e-05            0
           0            0            0            0            0    3.326e-06            0            0        5e-05
           0  -4.4145e-06            0  5.00974e-05            0            0   0.00100274            0            0
  4.4145e-06            0            0            0  5.00974e-05            0            0   0.00100274            0
           0            0            0            0            0        5e-05            0            0        0.001]
  noise model sigmas: 0.00316228 0.00316228 0.00316228 0.00182474 0.00182474 0.00182373  0.0316661  0.0316661  0.0316228

Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]
Factor error (0.5 * ||error||^2_Sigma): 0.0

Source¶