PreintegratedImuMeasurements

Overview¶

The PreintegratedImuMeasurements class (often abbreviated as PIM) is the standard container in GTSAM for accumulating high-rate IMU measurements (accelerometer and gyroscope) between two keyframes or time steps ( $t_i$ and $t_j$ ). It performs preintegration, effectively summarizing the relative motion $(\Delta R_{ij}, \Delta p_{ij}, \Delta v_{ij})$ predicted by the IMU measurements, while accounting for sensor noise and a fixed estimate of the IMU bias (biasHat_).

This preintegrated summary allows IMU information to be incorporated into a factor graph as a single factor (ImuFactor or ImuFactor2) between states at $t_i$ and $t_j$ , avoiding the need to add every single IMU measurement to the graph. It also stores the 9x9 covariance matrix (preintMeasCov_) associated with the uncertainty of the preintegrated state change.

It inherits from an underlying implementation (ManifoldPreintegration or TangentPreintegration, selected at compile time) which defines the specific mathematical approach used for integration.

Mathematical Background¶

The core idea is to integrate the IMU kinematic equations relative to the state at time $t_i$ , using the bias estimate biasHat_ held within the class. The exact integration formulas depend on the chosen implementation (manifold or tangent space), but conceptually:

\Delta R_{ij} = \prod_{k=i}^{j-1} \text{Exp}((\omega_k - b_{g, est}) \Delta t)

(1)

\Delta v_{ij} = \sum_{k=i}^{j-1} \Delta R_{ik} (a_k - b_{a, est}) \Delta t

(2)

\Delta p_{ij} = \sum_{k=i}^{j-1} [ \Delta v_{ik} \Delta t + \frac{1}{2} \Delta R_{ik} (a_k - b_{a, est}) \Delta t^2 ]

(3)

where $\omega_k$ and $a_k$ are the measurements at step k, $b_{g, est}$ and $b_{a, est}$ are the components of biasHat_, and $\Delta R_{ik}$ is the preintegrated rotation from $i$ to $k$ .

Crucially, the class also propagates the covariance of these Δ terms based on the IMU noise specified in PreintegrationParams.

Key Functionality / API¶

Constructor: PreintegratedImuMeasurements(params, biasHat): Creates an instance, requiring shared PreintegrationParams and the initial bias estimate (biasHat_) used for integration.
integrateMeasurement(measuredAcc, measuredOmega, dt): Adds a single IMU measurement pair and time delta, updating the internal preintegrated state and covariance.
resetIntegration(): Resets the accumulated measurements and time to zero, keeping the biasHat_.
resetIntegrationAndSetBias(newBiasHat): Resets integration and updates the internal biasHat_.
Accessors: deltaTij(), deltaRij(), deltaPij(), deltaVij(), preintMeasCov(), biasHat(). These return the accumulated values.
predict(state_i, current_bias): Predicts the state at time $t_j$ given the state at $t_i$ and the current best estimate of the bias (which might differ from biasHat_). This function applies the necessary first-order corrections for the change in bias.
biasCorrectedDelta(current_bias): Returns the 9D tangent-space representation of the preintegrated measurement, corrected for the difference between current_bias and biasHat_.
computeError(state_i, state_j, current_bias): Calculates the 9D error between the preintegrated prediction (using current_bias) and the actual state change (state_i to state_j). This is the core calculation used within ImuFactor::evaluateError.

Usage Example¶

Create parameters, create the PIM object with an initial bias estimate, integrate measurements, and potentially use predict.

from gtsam import PreintegrationParams, PreintegratedImuMeasurements, NavState
from gtsam.imuBias import ConstantBias
import numpy as np

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

# 2. Define the bias estimate used for preintegration
initial_bias_acc = np.array([0.01, -0.01, 0.02])
initial_bias_gyro = np.array([0.001, 0.002, -0.001])
bias_hat = ConstantBias(initial_bias_acc, initial_bias_gyro)

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

# 4. Integrate measurements (example loop)
dt = 0.01 # 100 Hz
num_measurements = 10
acc_meas = np.array([0.1, 0.2, -9.7]) # Example measurement (sensor frame)
gyro_meas = np.array([0.01, -0.02, 0.03]) # Example measurement (sensor frame)

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("Bias Hat Used:")
pim.biasHat().print()
print("Preintegration Covariance (9x9):\n", np.round(1000*pim.preintMeasCov(),2))

# 6. Example Prediction (requires initial state)
initial_state = NavState() # Default: Identity rotation, zero pos/vel
current_best_bias = bias_hat # Assume bias estimate hasn't changed yet
predicted_state = pim.predict(initial_state, current_best_bias)
print("\nPredicted State:")
predicted_state.print()

Total integration time: 0.09999999999999999
Delta R:
 [[ 9.99992775e-01 -3.10098211e-03 -2.19859941e-03]
 [ 3.09900212e-03  9.99994790e-01 -9.03407707e-04]
 [ 2.20138940e-03  8.96587715e-04  9.99997175e-01]]
Delta P: [ 0.00047953  0.00106289 -0.04859943]
Delta V: [ 0.00993257  0.02140713 -0.97198182]
Bias Hat Used:
acc =  0.01 -0.01  0.02 gyro =  0.001  0.002 -0.001
Preintegration Covariance (9x9):
 [[ 0.01  0.   -0.   -0.    0.    0.   -0.    0.    0.  ]
 [ 0.    0.01  0.   -0.   -0.   -0.   -0.   -0.   -0.  ]
 [-0.    0.    0.01 -0.    0.   -0.   -0.    0.   -0.  ]
 [-0.   -0.   -0.    0.   -0.    0.    0.05 -0.    0.  ]
 [ 0.   -0.    0.   -0.    0.    0.   -0.    0.05  0.  ]
 [ 0.   -0.   -0.    0.    0.    0.    0.    0.    0.05]
 [-0.   -0.   -0.    0.05 -0.    0.    1.   -0.    0.  ]
 [ 0.   -0.    0.   -0.    0.05  0.   -0.    1.    0.  ]
 [ 0.   -0.   -0.    0.    0.    0.05  0.    0.    1.  ]]

Predicted State:
R: [
	0.999993, -0.00310098, -0.0021986;
	0.003099, 0.999995, -0.000903408;
	0.00220139, 0.000896588, 0.999997
]
p: 0.000479535  0.00106289  -0.0976494
v: 0.00993257  0.0214071   -1.95298

The pim object is then passed to the ImuFactor or ImuFactor2 constructor.

Source¶

ImuFactor.h (Contains PIM definition)
ImuFactor.cpp
ManifoldPreintegration.h/.cpp (One possible implementation)
TangentPreintegration.h/.cpp (Another possible implementation)

GTSAM Docs

PreintegratedCombinedMeasurements

GTSAM Docs

PreintegratedRotation