CombinedImuFactor - GTSAM Docs

Overview¶

The CombinedImuFactor is an advanced IMU factor in GTSAM that offers several improvements over the standard ImuFactor:

Bias Evolution: It explicitly models the evolution of IMU biases between time steps $i$ and $j$ using a random walk model.
6-Way Factor: Consequently, it connects six variables: Pose_i, Vel_i, Bias_i, Pose_j, Vel_j, and Bias_j.
Combined Preintegration: It uses PreintegratedCombinedMeasurements which propagates a full 15x15 covariance matrix, accounting for correlations between the preintegrated state and the biases, as well as the bias random walk noise.

This factor is generally preferred when bias stability is a concern or when modeling the time-varying nature of biases is important for accuracy. It eliminates the need for separate BetweenFactors on bias variables.

Mathematical Formulation¶

The CombinedImuFactor has a 15-dimensional error vector, conceptually split into two parts:

Preintegration Error (9 dimensions): This part is identical in concept to the error in ImuFactor. It measures the discrepancy between the relative motion predicted by the PreintegratedCombinedMeasurements (using the current estimate of $b_i$ ) and the relative motion implied by the states $X_i = (R_i, p_i, v_i)$ and $X_j = (R_j, p_j, v_j)$ .
$e_{imu} = \text{NavState::Logmap}( \text{pim.predict}(X_i, b_i)^{-1} \otimes X_j )$
(1)
Bias Random Walk Error (6 dimensions): This part measures how much the change in bias ( $b_j - b_i$ ) deviates from the expected zero-mean random walk.
$e_{bias} = b_j - b_i$
(2)

The total error vector is $e = [e_{imu}; e_{bias}]$ .

The factor’s noise model (derived from pim.preintMeasCov()) is a 15x15 matrix that correctly weights these two error components, accounting for the propagated uncertainty from IMU measurements, initial bias uncertainty, and the bias random walk process noise.

Key Functionality / API¶

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

Usage Example¶

Create combined parameters, create a combined PIM object, define keys (including two bias keys), and construct the factor.

import numpy as np
from gtsam import (PreintegrationCombinedParams,
                   PreintegratedCombinedMeasurements, CombinedImuFactor,
                   NonlinearFactorGraph, Values, Pose3, NavState)
from gtsam.imuBias import ConstantBias
from gtsam.symbol_shorthand import X, V, B

# 1. Create Combined Parameters and PIM (as in PreintegratedCombinedMeasurements example)
params = PreintegrationCombinedParams.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_acc_rw_sigma = 0.001
bias_gyro_rw_sigma = 0.0001
params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)
params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)
initial_bias_cov = np.diag(np.full(6, 1e-3)) # Example initial bias uncertainty
params.setBiasAccOmegaInit(initial_bias_cov)
bias_hat = ConstantBias()
pim = PreintegratedCombinedMeasurements(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. Define Symbolic Keys using shorthand
# Keys: X(0), V(0), B(0) for time i
#       X(1), V(1), B(1) for time j

# 3. Create the CombinedImuFactor
# The 15x15 noise model is automatically derived from pim.preintMeasCov()
combined_imu_factor = CombinedImuFactor(
    X(0), V(0),
    X(1), V(1),
    B(0), B(1),
    pim)

print("Created CombinedImuFactor:")
combined_imu_factor.print()

# 4. Example: Evaluate error with perfect states & no bias change (should be near zero)
graph = NonlinearFactorGraph()
graph.add(combined_imu_factor)

values = Values()
pose_i = Pose3()
vel_i = np.zeros(3)
bias_i = ConstantBias() # Matches bias_hat used in PIM
bias_j = bias_i # Assume no bias change for zero error check

# Predict state j using the PIM
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(B(0), bias_i)
values.insert(X(1), pose_j)
values.insert(V(1), vel_j)
values.insert(B(1), bias_j)

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

Created CombinedImuFactor:
CombinedImuFactor(x0,v0,x1,v1,b0,b1)
  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 [      0.00011            0            0            0  1.53772e-06            0            0  4.85595e-05            0            0            0            0      4.5e-11            0            0
           0      0.00011            0 -1.53772e-06            0            0 -4.85595e-05            0            0            0            0            0            0      4.5e-11            0
           0            0      0.00011            0            0            0            0            0            0            0            0            0            0            0      4.5e-11
           0 -1.53772e-06            0  3.69908e-06            0            0  5.60718e-05            0            0    1.425e-10            0            0            0  -2.6487e-13            0
 1.53772e-06            0            0            0  3.69908e-06            0            0  5.60718e-05            0            0    1.425e-10            0   2.6487e-13            0            0
           0            0            0            0            0   3.6585e-06            0            0      5.5e-05            0            0    1.425e-10            0            0            0
           0 -4.85595e-05            0  5.60718e-05            0            0   0.00113017            0            0      4.5e-09            0            0            0  -1.1772e-11            0
 4.85595e-05            0            0            0  5.60718e-05            0            0   0.00113017            0            0      4.5e-09            0   1.1772e-11            0            0
           0            0            0            0            0      5.5e-05            0            0       0.0011            0            0      4.5e-09            0            0            0
           0            0            0    1.425e-10            0            0      4.5e-09            0            0        1e-07            0            0            0            0            0
           0            0            0            0    1.425e-10            0            0      4.5e-09            0            0        1e-07            0            0            0            0
           0            0            0            0            0    1.425e-10            0            0      4.5e-09            0            0        1e-07            0            0            0
     4.5e-11            0            0            0   2.6487e-13            0            0   1.1772e-11            0            0            0            0        1e-09            0            0
           0      4.5e-11            0  -2.6487e-13            0            0  -1.1772e-11            0            0            0            0            0            0        1e-09            0
           0            0      4.5e-11            0            0            0            0            0            0            0            0            0            0            0        1e-09 ]
  noise model: Gaussian [
	96.6351, 0, 0, 0, 91.8245, 0, -0, -8.70782, -0, 0, 0.261002, 0, -4.27039, 0, 0;
	0, 96.6351, 0, -91.8245, 0, 0, 8.70782, -0, -0, -0.261002, 0, 0, 0, -4.27039, 0;
	0, 0, 95.3463, 0, 0, 0, -0, -0, -0, 0, 0, 0, 0, 0, -4.29058;
	0, 0, 0, 1044.19, 0, 0, -51.806, -0, -0, 0.843301, 0, 0, 0, -0.333286, 0;
	0, 0, 0, 0, 1044.19, 0, -0, -51.806, -0, 0, 0.843301, 0, 0.333286, 0, 0;
	0, 0, 0, 0, 0, 1049.15, -0, -0, -52.4575, 0, 0, 0.865549, 0, 0, 0;
	0, 0, 0, 0, 0, 0, 29.746, -0, -0, -1.33857, 0, 0, 0, 0.35017, 0;
	0, 0, 0, 0, 0, 0, 0, 29.746, -0, 0, -1.33857, 0, -0.35017, 0, 0;
	0, 0, 0, 0, 0, 0, 0, 0, 30.1511, 0, 0, -1.3568, 0, 0, 0;
	0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, 0, 0, 5.26625e-20, 0;
	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, -5.26625e-20, 0, 0;
	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, 0, 0;
	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8, 0, 0;
	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8, 0;
	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8
]

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

Source¶

GTSAM Docs

BarometricFactor

GTSAM Docs

ConstantVelocityFactor