Skip to article frontmatterSkip to article content

PreintegratedCombinedMeasurements

Open In Colab

Overview

The PreintegratedCombinedMeasurements class is the preintegration container specifically designed for use with the CombinedImuFactor. Like PreintegratedImuMeasurements, it accumulates IMU measurements between two time steps (ti,tjt_i, t_j) using a fixed bias estimate (biasHat_).

However, it differs significantly in its covariance propagation:

  1. It uses PreintegrationCombinedParams, which include bias random walk parameters.
  2. It propagates a larger 15x15 covariance matrix (preintMeasCov_). This matrix captures the uncertainty of the preintegrated ΔR,Δp,Δv\Delta R, \Delta p, \Delta v (9x9 block), the uncertainty of the bias estimate itself (6x6 block), and crucially, the cross-correlations between the preintegrated measurements and the bias estimate.

Accounting for these correlations and the bias evolution noise allows the CombinedImuFactor to properly model the relationship between the states at ti,tjt_i, t_j and the biases at bi,bjb_i, b_j.

Mathematical Background

The mean propagation (calculating ΔR,Δp,Δv\Delta R, \Delta p, \Delta v) is mathematically identical to that in PreintegratedImuMeasurements, using the same underlying implementation (ManifoldPreintegration or TangentPreintegration).

The key difference lies in the covariance propagation. The update step for the 15x15 covariance matrix Σk→Σk+1\Sigma_k \rightarrow \Sigma_{k+1} incorporates not only the IMU measurement noise (like the standard PIM) but also:

  • The noise from the bias random walk process (defined in PreintegrationCombinedParams).
  • The uncertainty of the biasHat_ used during the integration step (via biasAccOmegaInit from the parameters).

This results in a more complex but statistically more accurate propagation of uncertainty, especially when biases are expected to drift significantly or have substantial initial uncertainty. The derivation details can be found in technical reports and papers related to the CombinedImuFactor (e.g., Carlone et al., IJRR 2015).

Key Functionality / API

The API is very similar to PreintegratedImuMeasurements:

  • Constructor: PreintegratedCombinedMeasurements(params, biasHat): Requires shared PreintegrationCombinedParams.
  • integrateMeasurement(measuredAcc, measuredOmega, dt): Adds a measurement, updating the internal state and the 15x15 covariance.
  • resetIntegration() / resetIntegrationAndSetBias(newBiasHat): Resets the integration.
  • Accessors: deltaTij(), deltaRij(), deltaPij(), deltaVij(), biasHat().
  • preintMeasCov(): Returns the 15x15 covariance matrix.
  • predict(state_i, current_bias): Predicts state XjX_j (same logic as standard PIM, using only the ΔR,p,v\Delta R, p, v parts).
  • biasCorrectedDelta(current_bias): Returns the 9D tangent-space vector corrected for bias difference (same logic as standard PIM).
  • Note: There isn’t a direct equivalent of computeError within this class, as the full error calculation (including the bias random walk part) is handled by the CombinedImuFactor itself.

Usage Example

Create combined parameters, create the object, integrate measurements.

from gtsam import PreintegrationCombinedParams, PreintegratedCombinedMeasurements
from gtsam.imuBias import ConstantBias
import numpy as np

# 1. Create Combined Parameters (as in PreintegrationCombinedParams 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)

# 2. Define the bias estimate used for preintegration
bias_hat = ConstantBias() # Start with zero bias estimate

# 3. Create the PreintegratedCombinedMeasurements object
pim = PreintegratedCombinedMeasurements(params, bias_hat)

# 4. Integrate measurements
dt = 0.01 # 100 Hz
num_measurements = 10
acc_meas = np.array([0.1, 0.0, -9.8]) 
gyro_meas = np.array([0.0, 0.0, 0.05])

for _ in range(num_measurements):
    pim.integrateMeasurement(acc_meas, gyro_meas, dt)

# 5. Inspect the results
print("Total integration time:", pim.deltaTij())
print("Delta R:\n", pim.deltaRij().matrix())
print("Delta P:", pim.deltaPij())
print("Delta V:", pim.deltaVij())
print("Preintegration Covariance (15x15 shape):", pim.preintMeasCov().shape)
# print("Preintegration Covariance:\n", pim.preintMeasCov()) # Might be large
Total integration time: 0.09999999999999999
Delta R:
 [[ 0.9999875  -0.00499998  0.        ]
 [ 0.00499998  0.9999875   0.        ]
 [ 0.          0.          1.        ]]
Delta P: [ 4.99999147e-04  7.12499187e-07 -4.90000000e-02]
Delta V: [ 9.99996438e-03  2.24999578e-05 -9.80000000e-01]
Preintegration Covariance (15x15 shape): (15, 15)

This pim object is then passed to the CombinedImuFactor constructor.

Source

GTSAM Docs
MagPoseFactor
GTSAM Docs
PreintegratedImuMeasurements