This notebook shows that the two ways of incorporating pre-integrated IMU data in GTSAM are mathematically equivalent as long as the same continuous-time noise is used:
gtsam.ImuFactor+gtsam.BetweenFactorConstantBias– bias random walk injected afterwards with a BetweenFactor.gtsam.CombinedImuFactor– folds that BetweenFactor into the IMU factor itself.
We build the two graphs side-by-side and show they produce identical posteriors.
Continuous-time Qc versus discrete-time Σ¶
IMU bias drift is modelled as Brownian motion with power-spectral density (PSD)
Over a finite interval the discrete covariance is
CombinedImuFactor– pass continuous viasetBiasAccCovariance()/setBiasOmegaCovariance(). The implementation multiplies it bydtinside everyintegrateMeasurement(dt), so the factor automatically accumulates after .ImuFactor+ Between-factor – do not add bias noise during pre-integration; instead attach a singleBetweenFactorConstantBiasspanning the same interval T and give it the discrete covariance (implemented by scaling the standard deviation by ).
Because both graphs end up with the same Σ, they contain the same information.
Practical tip – when is
CombinedImuFactorworth it?IMU biases drift slowly (seconds to minutes). If your key-frames are already that far apart, inserting one
BetweenFactorConstantBiasper key-frame is cheaper and accurate. ReserveCombinedImuFactorfor cases where you need bias estimates at every IMU integration interval.
Step 1 – Common setup and imports¶
# Install gtsam-develop if not installed
try:
import gtsam
except ImportError:
%pip install --quiet gtsam-develop# Imports
import gtsam
import numpy as np
from gtsam.symbol_shorthand import X, V, Bdef compare_matrices_side_by_side(mat1, mat2, precision=4):
mat1 = np.array(mat1)
mat2 = np.array(mat2)
rows = max(mat1.shape[0], mat2.shape[0])
m1 = np.array2string(mat1, precision=precision, suppress_small=True, max_line_width=80).splitlines()
m2 = np.array2string(mat2, precision=precision, suppress_small=True, max_line_width=80).splitlines()
for i in range(rows):
left = m1[i] if i < len(m1) else ""
right = m2[i] if i < len(m2) else ""
print(f"{left:<50} | {right}")We simulate a stationary IMU: only gravity accelerates the sensor and the true angular velocity is zero.
# Scenario parameters
dt = 0.1 # 10 msps between states
integration_time = 1.0 # each factor integrates 1 s (10×dt)
gravity = np.array([0, 0, -9.81])
# IMU measurements (zero motion)
measured_acc = -gravity # body-frame specific force
measured_gyro = np.array([0, 0, 0])Pre-integration parameters.
PreintegrationCombinedParams additionally needs the bias PSDs (continuous-time ); for the classic ImuFactor those are supplied later via the Between-factor.
# IMU sensor noise and bias random-walk PSDs (σ = sqrt(Qc))
accel_noise_sigma = 1e-4
gyro_noise_sigma = 1e-4
integration_noise_sigma = 1e-4
bias_acc_walk_sigma = 1e-4 # σ (m/s²)/√s
bias_gyro_walk_sigma = 1e-4 # σ (rad/s)/√s
# Classic ImuFactor params
params_imu = gtsam.PreintegrationParams(gravity)
params_imu.setAccelerometerCovariance(np.eye(3)*accel_noise_sigma**2)
params_imu.setGyroscopeCovariance (np.eye(3)*gyro_noise_sigma**2)
params_imu.setIntegrationCovariance (np.eye(3)*integration_noise_sigma**2)
# CombinedImuFactor params
params_combined = gtsam.PreintegrationCombinedParams(gravity)
params_combined.setAccelerometerCovariance(np.eye(3)*accel_noise_sigma**2)
params_combined.setGyroscopeCovariance (np.eye(3)*gyro_noise_sigma**2)
params_combined.setIntegrationCovariance (np.eye(3)*integration_noise_sigma**2)
# ***Continuous-time*** bias PSDs:
params_combined.setBiasAccCovariance (np.eye(3)*bias_acc_walk_sigma**2)
params_combined.setBiasOmegaCovariance(np.eye(3)*bias_gyro_walk_sigma**2)Step 2 – Pre-integration and covariance comparison¶
Now we create the preintegrated measurement objects (PIMs) and integrate our simulated measurements. We can then directly compare their preintegrated measurement covariances.
The classic PIM’s 9 × 9 covariance contains only sensor noise. The combined PIM’s 15 × 15 covariance also contains the accumulated bias-random-walk noise: therefore its top-left 9 × 9 block is larger – exactly by – and we explain later how the ImuFactor graph adds that same amount via the Between-factor.
# We start with a zero-bias assumption for preintegration
bias_0 = gtsam.imuBias.ConstantBias()
# Create the two PIM objects
pim_imu = gtsam.PreintegratedImuMeasurements (params_imu, bias_0)
pim_combined = gtsam.PreintegratedCombinedMeasurements(params_combined, bias_0)
# Integrate measurements
for _ in range(int(integration_time/dt)):
pim_imu.integrateMeasurement (measured_acc, measured_gyro, dt)
pim_combined.integrateMeasurement(measured_acc, measured_gyro, dt)
print("IMU-only 9×9 vs Combined 15×15 (×1e9):")
compare_matrices_side_by_side(1e9*pim_imu.preintMeasCov(),
1e9*pim_combined.preintMeasCov(), precision=0)IMU-only 9×9 vs Combined 15×15 (×1e9):
[[ 10. 0. 0. 0. -14. 0. 0. -44. 0.] | [[ 13. 0. 0. 0. -16. 0. 0. -53. 0. 0. 0. 0. 5. 0. 0.]
[ 0. 10. 0. 14. 0. 0. 44. 0. 0.] | [ 0. 13. 0. 16. 0. 0. 53. 0. 0. 0. 0. 0. 0. 5. 0.]
[ 0. 0. 10. 0. 0. 0. 0. 0. 0.] | [ 0. 0. 13. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 5.]
[ 0. 14. 0. 50. 0. 0. 102. 0. 0.] | [ 0. 16. 0. 52. 0. 0. 110. 0. 0. 1. 0. 0. 0. 3. 0.]
[-14. 0. 0. 0. 50. 0. 0. 102. 0.] | [-16. 0. 0. 0. 52. 0. 0. 110. 0. 0. 1. 0. -3. 0. 0.]
[ 0. 0. 0. 0. 0. 13. 0. 0. 5.] | [ 0. 0. 0. 0. 0. 14. 0. 0. 6. 0. 0. 1. 0. 0. 0.]
[ 0. 44. 0. 102. 0. 0. 284. 0. 0.] | [ 0. 53. 0. 110. 0. 0. 315. 0. 0. 5. 0. 0. 0. 12. 0.]
[-44. 0. 0. 0. 102. 0. 0. 284. 0.] | [-53. 0. 0. 0. 110. 0. 0. 315. 0. 0. 5. 0. -12. 0. 0.]
[ 0. 0. 0. 0. 0. 5. 0. 0. 10.]] | [ 0. 0. 0. 0. 0. 6. 0. 0. 13. 0. 0. 5. 0. 0. 0.]
| [ 0. 0. 0. 1. 0. 0. 5. 0. 0. 10. 0. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 1. 0. 0. 5. 0. 0. 10. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 0. 1. 0. 0. 5. 0. 0. 10. 0. 0. 0.]
| [ 5. 0. 0. 0. -3. 0. 0. -12. 0. 0. 0. 0. 10. 0. 0.]
| [ 0. 5. 0. 3. 0. 0. 12. 0. 0. 0. 0. 0. 0. 10. 0.]
| [ 0. 0. 5. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 10.]]
Step 3: Building Equivalent Factor Graphs¶
Next, we construct the two factor graphs for a trajectory with two IMU integration steps (three states at times k=0, 1, 2). We will add identical priors to both graphs.
# Initial state at t=0
pose_0 = gtsam.Pose3()
vel_0 = np.zeros(3)
# The bias_0 variable is the same as the one used for pre-integration above.
# Define noise models for priors
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3 + [0.01]*3))
velocity_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3))
bias_prior_sigma = 0.01 # Standard deviation for bias prior
bias_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([bias_prior_sigma]*6))
# The bias random walk noise is defined once for the BetweenFactor
bias_walk_noise_model = gtsam.noiseModel.Diagonal.Sigmas(
np.concatenate([np.full(3, bias_acc_walk_sigma), np.full(3, bias_gyro_walk_sigma)]) * np.sqrt(integration_time)
)graph_imu = gtsam.NonlinearFactorGraph()
graph_combined = gtsam.NonlinearFactorGraph()
# Add identical priors to both graphs
graph_imu.add(gtsam.PriorFactorPose3(X(0), pose_0, pose_noise))
graph_imu.add(gtsam.PriorFactorVector(V(0), vel_0, velocity_noise))
graph_imu.add(gtsam.PriorFactorConstantBias(B(0), bias_0, bias_noise))
graph_combined.add(gtsam.PriorFactorPose3(X(0), pose_0, pose_noise))
graph_combined.add(gtsam.PriorFactorVector(V(0), vel_0, velocity_noise))
graph_combined.add(gtsam.PriorFactorConstantBias(B(0), bias_0, bias_noise))
# Add factors for two steps (0->1 and 1->2)
for k in range(2):
# ImuFactor graph: Add ImuFactor and BetweenFactor separately
graph_imu.add(gtsam.ImuFactor(X(k), V(k), X(k + 1), V(k + 1), B(k), pim_imu))
graph_imu.add(gtsam.BetweenFactorConstantBias(B(k), B(k + 1), gtsam.imuBias.ConstantBias(), bias_walk_noise_model))
# CombinedImuFactor graph: Add a single combined factor
graph_combined.add(gtsam.CombinedImuFactor(X(k), V(k), X(k+1), V(k+1), B(k), B(k+1), pim_combined))
print('Graph 1 (ImuFactor + BetweenFactor):')
graph_imu.print('')
print('\nGraph 2 (CombinedImuFactor):')
graph_combined.print('')Graph 1 (ImuFactor + BetweenFactor):
size: 7
Factor 0: PriorFactor on x0
prior mean: R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 0
isotropic dim=6 sigma=0.01
Factor 1: PriorFactor on v0
prior mean: [
0;
0;
0
]
isotropic dim=3 sigma=0.01
Factor 2: PriorFactor on b0
prior mean: acc = 0 0 0 gyro = 0 0 0
isotropic dim=6 sigma=0.01
Factor 3:
ImuFactor(x0,v0,x1,v1,b0)
preintegrated measurements:
deltaTij = 1
deltaRij.ypr = ( 0 -0 0)
deltaPij = 0 0 4.905
deltaVij = 0 0 9.81
gyrobias = 0 0 0
acc_bias = 0 0 0
preintMeasCov
[ 1e-08 0 0 0 -1.39793e-08 0 0 -4.4145e-08 0
0 1e-08 0 1.39793e-08 0 0 4.4145e-08 0 0
0 0 1e-08 0 0 0 0 0 0
0 1.39793e-08 0 5.02147e-08 0 0 1.02439e-07 0 0
-1.39793e-08 0 0 0 5.02147e-08 0 0 1.02439e-07 0
0 0 0 0 0 1.3325e-08 0 0 5e-09
0 4.4145e-08 0 1.02439e-07 0 0 2.84273e-07 0 0
-4.4145e-08 0 0 0 1.02439e-07 0 0 2.84273e-07 0
0 0 0 0 0 5e-09 0 0 1e-08]
noise model sigmas: 0.0001 0.0001 0.0001 0.000224086 0.000224086 0.000115434 0.000533172 0.000533172 0.0001
Factor 4: BetweenFactor(b0,b1)
measured: acc = 0 0 0 gyro = 0 0 0
isotropic dim=6 sigma=0.0001
Factor 5:
ImuFactor(x1,v1,x2,v2,b1)
preintegrated measurements:
deltaTij = 1
deltaRij.ypr = ( 0 -0 0)
deltaPij = 0 0 4.905
deltaVij = 0 0 9.81
gyrobias = 0 0 0
acc_bias = 0 0 0
preintMeasCov
[ 1e-08 0 0 0 -1.39793e-08 0 0 -4.4145e-08 0
0 1e-08 0 1.39793e-08 0 0 4.4145e-08 0 0
0 0 1e-08 0 0 0 0 0 0
0 1.39793e-08 0 5.02147e-08 0 0 1.02439e-07 0 0
-1.39793e-08 0 0 0 5.02147e-08 0 0 1.02439e-07 0
0 0 0 0 0 1.3325e-08 0 0 5e-09
0 4.4145e-08 0 1.02439e-07 0 0 2.84273e-07 0 0
-4.4145e-08 0 0 0 1.02439e-07 0 0 2.84273e-07 0
0 0 0 0 0 5e-09 0 0 1e-08]
noise model sigmas: 0.0001 0.0001 0.0001 0.000224086 0.000224086 0.000115434 0.000533172 0.000533172 0.0001
Factor 6: BetweenFactor(b1,b2)
measured: acc = 0 0 0 gyro = 0 0 0
isotropic dim=6 sigma=0.0001
Graph 2 (CombinedImuFactor):
size: 5
Factor 0: PriorFactor on x0
prior mean: R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 0
isotropic dim=6 sigma=0.01
Factor 1: PriorFactor on v0
prior mean: [
0;
0;
0
]
isotropic dim=3 sigma=0.01
Factor 2: PriorFactor on b0
prior mean: acc = 0 0 0 gyro = 0 0 0
isotropic dim=6 sigma=0.01
Factor 3:
CombinedImuFactor(x0,v0,x1,v1,b0,b1)
preintegrated measurements:
deltaTij = 1
deltaRij.ypr = ( 0 -0 0)
deltaPij = 0 0 4.905
deltaVij = 0 0 9.81
gyrobias = 0 0 0
acc_bias = 0 0 0
preintMeasCov [ 1.285e-08 0 0 0 -1.60129e-08 0 0 -5.26797e-08 0 0 0 0 4.5e-09 0 0
0 1.285e-08 0 1.60129e-08 0 0 5.26797e-08 0 0 0 0 0 0 4.5e-09 0
0 0 1.285e-08 0 0 0 0 0 0 0 0 0 0 0 4.5e-09
0 1.60129e-08 0 5.23698e-08 0 0 1.10381e-07 0 0 1.425e-09 0 0 0 2.6487e-09 0
-1.60129e-08 0 0 0 5.23698e-08 0 0 1.10381e-07 0 0 1.425e-09 0 -2.6487e-09 0 0
0 0 0 0 0 1.37083e-08 0 0 6.0125e-09 0 0 1.425e-09 0 0 0
0 5.26797e-08 0 1.10381e-07 0 0 3.14954e-07 0 0 4.5e-09 0 0 0 1.1772e-08 0
-5.26797e-08 0 0 0 1.10381e-07 0 0 3.14954e-07 0 0 4.5e-09 0 -1.1772e-08 0 0
0 0 0 0 0 6.0125e-09 0 0 1.285e-08 0 0 4.5e-09 0 0 0
0 0 0 1.425e-09 0 0 4.5e-09 0 0 1e-08 0 0 0 0 0
0 0 0 0 1.425e-09 0 0 4.5e-09 0 0 1e-08 0 0 0 0
0 0 0 0 0 1.425e-09 0 0 4.5e-09 0 0 1e-08 0 0 0
4.5e-09 0 0 0 -2.6487e-09 0 0 -1.1772e-08 0 0 0 0 1e-08 0 0
0 4.5e-09 0 2.6487e-09 0 0 1.1772e-08 0 0 0 0 0 0 1e-08 0
0 0 4.5e-09 0 0 0 0 0 0 0 0 0 0 0 1e-08 ]
noise model: Gaussian [
18259.6, 0, 0, 0, -2811.03, 0, -0, 3895.04, 0, 0, 1352.2, 0, 4376.12, 0, 0;
0, 18259.6, 0, 2811.03, 0, 0, -3895.04, 0, 0, -1352.2, 0, 0, 0, 4376.12, 0;
0, 0, 9611.39, 0, 0, 0, -0, 0, 0, 0, 0, 0, 0, 0, 4325.12;
0, 0, 0, 8621.36, 0, 0, -3073.58, 0, 0, -154.568, 0, 0, 0, -1334.68, 0;
0, 0, 0, 0, 8621.36, 0, -0, -3073.58, 0, 0, -154.568, 0, 1334.68, 0, 0;
0, 0, 0, 0, 0, 9604.69, -0, 0, -4765.74, 0, 0, -775.917, 0, 0, 0;
0, 0, 0, 0, 0, 0, 1828.57, 0, 0, 822.858, 0, 0, 0, 2152.6, 0;
0, 0, 0, 0, 0, 0, 0, 1828.57, 0, 0, 822.858, 0, -2152.6, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 9611.39, 0, 0, 4325.12, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0, 1.39698e-13, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, -1.39698e-13, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000
]
Factor 4:
CombinedImuFactor(x1,v1,x2,v2,b1,b2)
preintegrated measurements:
deltaTij = 1
deltaRij.ypr = ( 0 -0 0)
deltaPij = 0 0 4.905
deltaVij = 0 0 9.81
gyrobias = 0 0 0
acc_bias = 0 0 0
preintMeasCov [ 1.285e-08 0 0 0 -1.60129e-08 0 0 -5.26797e-08 0 0 0 0 4.5e-09 0 0
0 1.285e-08 0 1.60129e-08 0 0 5.26797e-08 0 0 0 0 0 0 4.5e-09 0
0 0 1.285e-08 0 0 0 0 0 0 0 0 0 0 0 4.5e-09
0 1.60129e-08 0 5.23698e-08 0 0 1.10381e-07 0 0 1.425e-09 0 0 0 2.6487e-09 0
-1.60129e-08 0 0 0 5.23698e-08 0 0 1.10381e-07 0 0 1.425e-09 0 -2.6487e-09 0 0
0 0 0 0 0 1.37083e-08 0 0 6.0125e-09 0 0 1.425e-09 0 0 0
0 5.26797e-08 0 1.10381e-07 0 0 3.14954e-07 0 0 4.5e-09 0 0 0 1.1772e-08 0
-5.26797e-08 0 0 0 1.10381e-07 0 0 3.14954e-07 0 0 4.5e-09 0 -1.1772e-08 0 0
0 0 0 0 0 6.0125e-09 0 0 1.285e-08 0 0 4.5e-09 0 0 0
0 0 0 1.425e-09 0 0 4.5e-09 0 0 1e-08 0 0 0 0 0
0 0 0 0 1.425e-09 0 0 4.5e-09 0 0 1e-08 0 0 0 0
0 0 0 0 0 1.425e-09 0 0 4.5e-09 0 0 1e-08 0 0 0
4.5e-09 0 0 0 -2.6487e-09 0 0 -1.1772e-08 0 0 0 0 1e-08 0 0
0 4.5e-09 0 2.6487e-09 0 0 1.1772e-08 0 0 0 0 0 0 1e-08 0
0 0 4.5e-09 0 0 0 0 0 0 0 0 0 0 0 1e-08 ]
noise model: Gaussian [
18259.6, 0, 0, 0, -2811.03, 0, -0, 3895.04, 0, 0, 1352.2, 0, 4376.12, 0, 0;
0, 18259.6, 0, 2811.03, 0, 0, -3895.04, 0, 0, -1352.2, 0, 0, 0, 4376.12, 0;
0, 0, 9611.39, 0, 0, 0, -0, 0, 0, 0, 0, 0, 0, 0, 4325.12;
0, 0, 0, 8621.36, 0, 0, -3073.58, 0, 0, -154.568, 0, 0, 0, -1334.68, 0;
0, 0, 0, 0, 8621.36, 0, -0, -3073.58, 0, 0, -154.568, 0, 1334.68, 0, 0;
0, 0, 0, 0, 0, 9604.69, -0, 0, -4765.74, 0, 0, -775.917, 0, 0, 0;
0, 0, 0, 0, 0, 0, 1828.57, 0, 0, 822.858, 0, 0, 0, 2152.6, 0;
0, 0, 0, 0, 0, 0, 0, 1828.57, 0, 0, 822.858, 0, -2152.6, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 9611.39, 0, 0, 4325.12, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0, 1.39698e-13, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, -1.39698e-13, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000
]
Step 4: Creating Initial Estimates¶
We need to provide an initial guess for the optimizer. Since the problem is simple (no motion), we can initialize all states and biases to zero. The initial estimates for both graphs will be identical.
initial_estimate = gtsam.Values()
for k in range(3):
initial_estimate.insert(X(k), pose_0)
initial_estimate.insert(V(k), vel_0)
initial_estimate.insert(B(k), bias_0)
print('Initial Estimate:\n', initial_estimate)Initial Estimate:
Values with 9 values:
Value b0: (gtsam::imuBias::ConstantBias)
acc = 0 0 0 gyro = 0 0 0
Value b1: (gtsam::imuBias::ConstantBias)
acc = 0 0 0 gyro = 0 0 0
Value b2: (gtsam::imuBias::ConstantBias)
acc = 0 0 0 gyro = 0 0 0
Value v0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
0
]
Value v1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
0
]
Value v2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
0
]
Value x0: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 0
Value x1: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 0
Value x2: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 0
Step 5: Comparing Factor Jacobians¶
Before optimizing, we can inspect the factors themselves. A nonlinear factor’s linearize method produces a JacobianFactor, which is a linear approximation of the factor’s error function around a given state. However, it does mix in the PIM covariances, which we do know are different from Step 2. Hence, here we compare the linearization using numerical derivatives of evaluateError, which gives us the unwhitened error.
Below we show that the combination of Jacobians from ImuFactor and BetweenFactor for the interval k=0 to k=1 is identical to the single Jacobian from CombinedImuFactor, at least for the first 9 rows, which correspond to the NavState error.
imu_factor_01 = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim_imu)
combined_factor_01 = gtsam.CombinedImuFactor(X(0), V(0), X(1), V(1), B(0), B(1), pim_combined)For the first four arguments (really NavState 0 and NavState 1) the Jacobians are indeed identical:
from gtsam.utils.numerical_derivative import numericalDerivative51, numericalDerivative61
print('Comparing Jacobians w.r.t. pose:')
H1 = numericalDerivative51(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)
H2 = numericalDerivative61(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)
compare_matrices_side_by_side(H1, H2, precision=3)Comparing Jacobians w.r.t. pose:
[[ 1. 0. 0. 0. 0. 0. ] | [[ 1. 0. 0. 0. 0. 0. ]
[ 0. 1. 0. 0. 0. 0. ] | [ 0. 1. 0. 0. 0. 0. ]
[ 0. 0. 1. 0. 0. 0. ] | [ 0. 0. 1. 0. 0. 0. ]
[ 0. 4.905 0. 1. 0. 0. ] | [ 0. 4.905 0. 1. 0. 0. ]
[-4.905 0. 0. 0. 1. 0. ] | [-4.905 0. 0. 0. 1. 0. ]
[ 0. 0. 0. 0. 0. 1. ] | [ 0. 0. 0. 0. 0. 1. ]
[ 0. 9.81 0. 0. 0. 0. ] | [ 0. 9.81 0. 0. 0. 0. ]
[-9.81 0. 0. 0. 0. 0. ] | [-9.81 0. 0. 0. 0. 0. ]
[ 0. 0. 0. 0. 0. 0. ]] | [ 0. 0. 0. 0. 0. 0. ]
| [ 0. 0. 0. 0. 0. 0. ]
| [ 0. 0. 0. 0. 0. 0. ]
| [ 0. 0. 0. 0. 0. 0. ]
| [ 0. 0. 0. 0. 0. 0. ]
| [ 0. 0. 0. 0. 0. 0. ]
| [ 0. 0. 0. 0. 0. 0. ]]
from gtsam.utils.numerical_derivative import numericalDerivative52, numericalDerivative62
print('Comparing Jacobians w.r.t. vel:')
H1 = numericalDerivative52(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)
H2 = numericalDerivative62(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)
compare_matrices_side_by_side(H1, H2, precision=3)Comparing Jacobians w.r.t. vel:
[[0. 0. 0.] | [[0. 0. 0.]
[0. 0. 0.] | [0. 0. 0.]
[0. 0. 0.] | [0. 0. 0.]
[1. 0. 0.] | [1. 0. 0.]
[0. 1. 0.] | [0. 1. 0.]
[0. 0. 1.] | [0. 0. 1.]
[1. 0. 0.] | [1. 0. 0.]
[0. 1. 0.] | [0. 1. 0.]
[0. 0. 1.]] | [0. 0. 1.]
| [0. 0. 0.]
| [0. 0. 0.]
| [0. 0. 0.]
| [0. 0. 0.]
| [0. 0. 0.]
| [0. 0. 0.]]
from gtsam.utils.numerical_derivative import numericalDerivative53, numericalDerivative63
print('Comparing Jacobians w.r.t. second pose:')
H1 = numericalDerivative53(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)
H2 = numericalDerivative63(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)
compare_matrices_side_by_side(H1, H2, precision=3)Comparing Jacobians w.r.t. second pose:
[[-1. 0. 0. 0. 0. 0.] | [[-1. 0. 0. 0. 0. 0.]
[ 0. -1. 0. 0. 0. 0.] | [ 0. -1. 0. 0. 0. 0.]
[ 0. 0. -1. 0. 0. 0.] | [ 0. 0. -1. 0. 0. 0.]
[ 0. -0. 0. -1. 0. 0.] | [ 0. -0. 0. -1. 0. 0.]
[ 0. 0. 0. 0. -1. 0.] | [ 0. 0. 0. 0. -1. 0.]
[ 0. 0. 0. 0. 0. -1.] | [ 0. 0. 0. 0. 0. -1.]
[ 0. -0. 0. 0. 0. 0.] | [ 0. -0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.] | [ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]] | [ 0. 0. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 0. 0.]
| [ 0. 0. 0. 0. 0. 0.]]
from gtsam.utils.numerical_derivative import numericalDerivative54, numericalDerivative64
print('Comparing Jacobians w.r.t. second velocity:')
H1 = numericalDerivative54(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)
H2 = numericalDerivative64(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)
compare_matrices_side_by_side(H1, H2, precision=3)Comparing Jacobians w.r.t. second velocity:
[[ 0. 0. 0.] | [[ 0. 0. 0.]
[ 0. 0. 0.] | [ 0. 0. 0.]
[ 0. 0. 0.] | [ 0. 0. 0.]
[ 0. 0. 0.] | [ 0. 0. 0.]
[ 0. 0. 0.] | [ 0. 0. 0.]
[ 0. 0. 0.] | [ 0. 0. 0.]
[-1. 0. 0.] | [-1. 0. 0.]
[ 0. -1. 0.] | [ 0. -1. 0.]
[ 0. 0. -1.]] | [ 0. 0. -1.]
| [ 0. 0. 0.]
| [ 0. 0. 0.]
| [ 0. 0. 0.]
| [ 0. 0. 0.]
| [ 0. 0. 0.]
| [ 0. 0. 0.]]
For the 5th argument, the initial bias, we see that again the first 9 rows are identical, but obviously the 6 last rows correspond to the bias evolution, and its Jacobian is in the first bias.
from gtsam.utils.numerical_derivative import numericalDerivative55, numericalDerivative65
print('Comparing Jacobians w.r.t. bias B(0):')
H1 = numericalDerivative55(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)
H2 = numericalDerivative65(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)
compare_matrices_side_by_side(H1, H2, precision=3)Comparing Jacobians w.r.t. bias B(0):
[[ 0. 0. 0. -1. 0. 0. ] | [[ 0. 0. 0. -1. 0. 0. ]
[ 0. 0. 0. 0. -1. 0. ] | [ 0. 0. 0. 0. -1. 0. ]
[ 0. 0. 0. 0. 0. -1. ] | [ 0. 0. 0. 0. 0. -1. ]
[-0.5 0. 0. 0. -1.398 0. ] | [-0.5 0. 0. 0. -1.398 0. ]
[ 0. -0.5 0. 1.398 0. 0. ] | [ 0. -0.5 0. 1.398 0. 0. ]
[ 0. 0. -0.5 0. 0. 0. ] | [ 0. 0. -0.5 0. 0. 0. ]
[-1. 0. 0. 0. -4.414 0. ] | [-1. 0. 0. 0. -4.414 0. ]
[ 0. -1. 0. 4.414 0. 0. ] | [ 0. -1. 0. 4.414 0. 0. ]
[ 0. 0. -1. 0. 0. 0. ]] | [ 0. 0. -1. 0. 0. 0. ]
| [ 1. 0. 0. 0. 0. 0. ]
| [ 0. 1. 0. 0. 0. 0. ]
| [ 0. 0. 1. 0. 0. 0. ]
| [ 0. 0. 0. 1. 0. 0. ]
| [ 0. 0. 0. 0. 1. 0. ]
| [ 0. 0. 0. 0. 0. 1. ]]
The derivative in B(1) is negative :
from gtsam.utils.numerical_derivative import numericalDerivative66
H2 = numericalDerivative66(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)
print(f'Note: The IMU factor does not have a B(1) term, so we only show the combined factor:\n{H2}')Note: The IMU factor does not have a B(1) term, so we only show the combined factor:
[[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[ 0. 0. 0. 0. 0. 0.]
[-1. 0. 0. 0. 0. 0.]
[ 0. -1. 0. 0. 0. 0.]
[ 0. 0. -1. 0. 0. 0.]
[ 0. 0. 0. -1. 0. 0.]
[ 0. 0. 0. 0. -1. 0.]
[ 0. 0. 0. 0. 0. -1.]]
Step 6: Optimizing and Comparing Final Results¶
Now that we’ve established the underlying mathematical equivalence of the factors, let’s optimize both graphs and compare the final results. They should be identical.
# Optimize both graphs
optimizer_imu = gtsam.LevenbergMarquardtOptimizer(graph_imu, initial_estimate)
result_imu = optimizer_imu.optimize()
optimizer_combined = gtsam.LevenbergMarquardtOptimizer(graph_combined, initial_estimate)
result_combined = optimizer_combined.optimize()
print('--- Results from ImuFactor + BetweenFactor Graph ---')
result_imu.print('')
print('\n--- Results from CombinedImuFactor Graph ---')
result_combined.print('')
# Verify the results are equal
assert(result_imu.equals(result_combined, 1e-6))
print('\nSuccess! The final results from both optimizers are equal.')--- Results from ImuFactor + BetweenFactor Graph ---
Values with 9 values:
Value b0: (gtsam::imuBias::ConstantBias)
acc = 0 0 1.6433e-23 gyro = 0 0 0
Value b1: (gtsam::imuBias::ConstantBias)
acc = 0 0 1.64335e-23 gyro = 0 0 0
Value b2: (gtsam::imuBias::ConstantBias)
acc = 0 0 1.64335e-23 gyro = 0 0 0
Value v0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
-1.33133e-23
]
Value v1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
1.77636e-15
]
Value v2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
3.55271e-15
]
Value x0: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 -4.43817e-24
Value x1: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 8.88178e-16
Value x2: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 3.55271e-15
--- Results from CombinedImuFactor Graph ---
Values with 9 values:
Value b0: (gtsam::imuBias::ConstantBias)
acc = 0 0 1.64055e-23 gyro = 0 0 0
Value b1: (gtsam::imuBias::ConstantBias)
acc = 0 0 1.64065e-23 gyro = 0 0 0
Value b2: (gtsam::imuBias::ConstantBias)
acc = 0 0 1.64067e-23 gyro = 0 0 0
Value v0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
-1.32995e-23
]
Value v1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
1.77636e-15
]
Value v2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
0;
0;
3.55271e-15
]
Value x0: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 -4.43611e-24
Value x1: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 8.88178e-16
Value x2: (gtsam::Pose3)
R: [
1, 0, 0;
0, 1, 0;
0, 0, 1
]
t: 0 0 3.55271e-15
Success! The final results from both optimizers are equal.
Finally, we can check the posterior marginal covariances on the bias estimates. As expected, these are also identical.
# Analyze marginal covariances
marginals_imu = gtsam.Marginals(graph_imu, result_imu)
marginals_combined = gtsam.Marginals(graph_combined, result_combined)
for k in range(3):
cov_imu_k = marginals_imu.marginalCovariance(B(k))
cov_combined_k = marginals_combined.marginalCovariance(B(k))
print(f'\nCovariance for Bias B({k}):')
compare_matrices_side_by_side(1e6*cov_imu_k, 1e6*cov_combined_k, precision=3)
Covariance for Bias B(0):
[[100. 0. 0. 0. 0. 0.] | [[100. 0. 0. 0. 0. 0.]
[ 0. 100. 0. -0. 0. 0.] | [ 0. 100. 0. -0. 0. 0.]
[ 0. 0. 100. 0. 0. 0.] | [ 0. 0. 100. 0. 0. 0.]
[ 0. -0. 0. 100. 0. 0.] | [ 0. -0. 0. 100. 0. 0.]
[ 0. 0. 0. 0. 100. 0.] | [ 0. 0. 0. 0. 100. 0.]
[ 0. 0. 0. 0. 0. 100.]] | [ 0. 0. 0. 0. 0. 100.]]
Covariance for Bias B(1):
[[100.01 0. 0. 0. -0. 0. ] | [[100.01 0. 0. 0. 0. 0. ]
[ 0. 100.01 0. 0. 0. 0. ] | [ 0. 100.01 0. -0. 0. 0. ]
[ 0. 0. 100.01 0. 0. 0. ] | [ 0. 0. 100.01 0. 0. 0. ]
[ 0. 0. 0. 100.01 0. 0. ] | [ 0. -0. 0. 100.01 0. 0. ]
[ -0. 0. 0. 0. 100.01 0. ] | [ 0. 0. 0. 0. 100.01 0. ]
[ 0. 0. 0. 0. 0. 100.01]] | [ 0. 0. 0. 0. 0. 100.01]]
Covariance for Bias B(2):
[[100.02 0. 0. 0. -0. 0. ] | [[100.02 0. 0. 0. 0. 0. ]
[ 0. 100.02 0. 0. 0. 0. ] | [ 0. 100.02 0. -0. 0. 0. ]
[ 0. 0. 100.02 0. 0. 0. ] | [ 0. 0. 100.02 0. 0. 0. ]
[ 0. 0. 0. 100.02 0. 0. ] | [ 0. -0. 0. 100.02 0. 0. ]
[ -0. 0. 0. 0. 100.02 0. ] | [ 0. 0. 0. 0. 100.02 0. ]
[ 0. 0. 0. 0. 0. 100.02]] | [ 0. 0. 0. 0. 0. 100.02]]
# Compare marginal covariances for poses and velocities between both graphs
for k in range(3):
cov_pose_imu = marginals_imu.marginalCovariance(X(k))
cov_pose_combined = marginals_combined.marginalCovariance(X(k))
print(f'\nCovariance for Pose X({k}):')
compare_matrices_side_by_side(1e6*cov_pose_imu, 1e6*cov_pose_combined, precision=3)
cov_vel_imu = marginals_imu.marginalCovariance(V(k))
cov_vel_combined = marginals_combined.marginalCovariance(V(k))
print(f'\nCovariance for Velocity V({k}):')
compare_matrices_side_by_side(1e6*cov_vel_imu, 1e6*cov_vel_combined, precision=3)
Covariance for Pose X(0):
[[100. 0. 0. 0. -0. 0.] | [[100. 0. 0. 0. 0. 0.]
[ 0. 100. 0. 0. 0. 0.] | [ 0. 100. 0. 0. 0. 0.]
[ 0. 0. 100. 0. 0. 0.] | [ 0. 0. 100. 0. 0. 0.]
[ 0. 0. 0. 100. 0. 0.] | [ 0. 0. 0. 100. 0. 0.]
[ -0. 0. 0. 0. 100. 0.] | [ 0. 0. 0. 0. 100. 0.]
[ 0. 0. 0. 0. 0. 100.]] | [ 0. 0. 0. 0. 0. 100.]]
Covariance for Velocity V(0):
[[100. 0. 0.] | [[100. 0. 0.]
[ 0. 100. 0.] | [ 0. 100. 0.]
[ 0. 0. 100.]] | [ 0. 0. 100.]]
Covariance for Pose X(1):
[[ 200.01 0. 0. 0. -630.306 0. ] | [[ 200.013 0. 0. 0. -630.309 0. ]
[ 0. 200.01 0. 630.306 0. 0. ] | [ 0. 200.013 0. 630.309 0. 0. ]
[ 0. 0. 200.01 0. 0. 0. ] | [ 0. 0. 200.013 0. 0. 0. ]
[ 0. 630.306 0. 2826.372 0. 0. ] | [ 0. 630.309 0. 2826.374 0. 0. ]
[-630.306 0. 0. 0. 2826.372 0. ] | [-630.309 0. 0. 0. 2826.374 0. ]
[ 0. 0. 0. 0. 0. 225.013]] | [ 0. 0. 0. 0. 0. 225.014]]
Covariance for Velocity V(1):
[[11772.675 0. 0. ] | [[11772.706 0. 0. ]
[ 0. 11772.675 0. ] | [ 0. 11772.706 0. ]
[ 0. 0. 200.01 ]] | [ 0. 0. 200.013]]
Covariance for Pose X(2):
[[ 500.03 0. 0. 0. -4385.205 0. ] | [[ 500.045 0. 0. 0. -4385.275 0. ]
[ 0. 500.03 0. 4385.205 0. 0. ] | [ 0. 500.045 0. 4385.274 0. 0. ]
[ 0. 0. 500.03 0. 0. 0. ] | [ 0. 0. 500.045 0. 0. 0. ]
[ 0. 4385.205 0. 54074.033 0. 0. ] | [ 0. 4385.274 0. 54074.364 0. 0. ]
[-4385.205 0. 0. 0. 54074.033 0. ] | [-4385.275 0. 0. 0. 54074.364 0. ]
[ 0. 0. 0. 0. 0. 900.049]] | [ 0. 0. 0. 0. 0. 900.061]]
Covariance for Velocity V(2):
[[73738.274 0. 0. ] | [[73739.28 0. 0. ]
[ 0. 73738.274 0. ] | [ 0. 73739.28 0. ]
[ 0. 0. 500.03 ]] | [ 0. 0. 500.045]]
Conclusion¶
We have successfully demonstrated that gtsam.CombinedImuFactor is a mathematically equivalent but more convenient way to model IMU measurements compared to using a separate gtsam.ImuFactor and gtsam.BetweenFactorConstantBias. We showed this by verifying that:
Their preintegrated measurement covariances are consistent.
The linearization (Jacobians) of a
CombinedImuFactoris identical to the combined Jacobians of anImuFactorand aBetweenFactor.The final optimized results and posterior covariances from factor graphs built with both methods are identical.
The explanation is that,
with the same continuous-time PSD both formulations propagate the same bias noise:
CombinedImuFactordoes it inside the PIM, whereas the classicImuFactorrelies on aBetweenFactorConstantBiaswhose covariance is ;consequently their Jacobians, optimized estimates and posterior covariances are identical (up to round-off), as the notebook demonstrates;
Final note: because biases evolve much more slowly than pose/velocity, using the simpler ImuFactor + Between-factor is usually sufficient and faster. Reach for CombinedImuFactor only when you truly need highly accurate bias estimates at every interval.