PreintegrationCombinedParams

Overview¶

The PreintegrationCombinedParams class holds parameters specifically required for the CombinedImuFactor and its associated PreintegratedCombinedMeasurements class.

It inherits from PreintegrationParams (and thus also PreintegratedRotationParams) and adds parameters that model the evolution of the IMU bias over time, typically as a random walk process. This is essential for the CombinedImuFactor, which estimates biases at both the start ( $b_i$ ) and end ( $b_j$ ) of the preintegration interval.

Key Parameters¶

In addition to all parameters from PreintegrationParams, this class adds:

biasAccCovariance: A 3x3 matrix representing the continuous-time covariance of the random walk process driving the accelerometer bias. Units: (m²/s⁵)/Hz ? (Check docs, represents variance growth rate).
biasOmegaCovariance: A 3x3 matrix representing the continuous-time covariance of the random walk process driving the gyroscope bias. Units: (rad²/s³)/Hz ? (Check docs, represents variance growth rate).
biasAccOmegaInit: A 6x6 matrix representing the covariance of the uncertainty in the initial bias estimate provided to the preintegration. This accounts for the fact that the bias used for integration (biasHat_) is itself uncertain.

Mathematical Background: Bias Random Walk¶

The CombinedImuFactor implicitly assumes that the bias evolves between time steps according to a random walk:

b_{k+1} = b_k + w_k, \quad w_k \sim \mathcal{N}(0, Q_b \Delta t)

(1)

where $b_k = [b_{a,k}; b_{g,k}]$ is the 6D bias vector at time $k$ , $w_k$ is zero-mean Gaussian noise, and $Q_b$ is the block-diagonal continuous-time covariance matrix formed from biasAccCovariance and biasOmegaCovariance:

Q_b = \begin{bmatrix} \text{biasAccCovariance} & 0 \\ 0 & \text{biasOmegaCovariance} \end{bmatrix}

(2)

The factor uses this model (integrated over the interval $\Delta t_{ij}$ ) to constrain the difference between $b_i$ and $b_j$ .

Key Functionality / API¶

Constructors:
- PreintegrationCombinedParams(n_gravity): Main constructor.
- MakeSharedD(g=9.81) / MakeSharedU(g=9.81): Convenience static methods for NED/ENU frames.
Setters: Methods like setBiasAccCovariance, setBiasOmegaCovariance, setBiasAccOmegaInit, plus all setters inherited from PreintegrationParams.
Getters: Corresponding getters for the combined parameters, plus all inherited getters.
print / equals: Standard methods.

Usage Example¶

Create parameters, often starting from the base PreintegrationParams settings, and then set the bias evolution parameters.

from gtsam import PreintegrationCombinedParams
import numpy as np

# 1. Create parameters for an ENU navigation frame (Z-up)
params = PreintegrationCombinedParams.MakeSharedU(9.81)

# 2. Set standard noise parameters (accel, gyro, integration)
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)

# 3. Set bias random walk noise parameters (example values)
bias_acc_rw_sigma = 0.001 # m/s^2 / sqrt(s) -> Covariance = sigma^2
bias_gyro_rw_sigma = 0.0001 # rad/s / sqrt(s) -> Covariance = sigma^2
params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)
params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)

# 4. Set initial bias uncertainty (covariance of bias used for preintegration)
# Example: Assume bias estimate used for preintegration has some uncertainty
initial_bias_acc_sigma = 0.1
initial_bias_gyro_sigma = 0.05
initial_bias_cov = np.diag(np.concatenate([
    np.full(3, initial_bias_acc_sigma**2),
    np.full(3, initial_bias_gyro_sigma**2)
]))
params.setBiasAccOmegaInit(initial_bias_cov)

print("Combined IMU Preintegration Parameters:")
params.print()

Combined IMU Preintegration Parameters:

gyroscopeCovariance:
[
0.0001      0      0
     0 0.0001      0
     0      0 0.0001
]
accelerometerCovariance:
[
0.01    0    0
   0 0.01    0
   0    0 0.01
]
integrationCovariance:
[
1e-08     0     0
    0 1e-08     0
    0     0 1e-08
]
n_gravity = (    0     0 -9.81)
biasAccCovariance:
[
1e-06     0     0
    0 1e-06     0
    0     0 1e-06
]
biasOmegaCovariance:
[
1e-08     0     0
    0 1e-08     0
    0     0 1e-08
]
biasAccOmegaInt:
[
  0.01      0      0      0      0      0
     0   0.01      0      0      0      0
     0      0   0.01      0      0      0
     0      0      0 0.0025      0      0
     0      0      0      0 0.0025      0
     0      0      0      0      0 0.0025
]

These combined parameters are then passed to PreintegratedCombinedMeasurements.

Source¶

PreintegrationCombinedParams.h
CombinedImuFactor.cpp (Contains the definition of this class)

GTSAM Docs

PreintegratedRotation

GTSAM Docs

PreintegrationParams