Navigation

The navigation module in GTSAM provides specialized tools for inertial navigation, GPS integration, and sensor fusion. Here’s an overview of key components organized by category:

Classes¶

Core Navigation Types¶

• NavState: Represents the complete navigation state $\mathcal{SE}_2(3)$, i.e., attitude, position, and velocity. It also implements the group ${SE}_2(3)$. See the NavState user guide.

• ImuBias: Models constant biases in IMU measurements (accelerometer and gyroscope).

Invariant Kalman Filtering¶

• ManifoldEKF: Implements an EKF for states that operate on a differentiable manifold.

• LieGroupEKF: Implements an EKF for states that operate on a Lie group with state dependent dynamics.

• InvariantEKF: Implements an EKF for states that operate on a Lie group with group composition (state independent) dynamics. See the InvariantEKF user guide.

Attitude Estimation¶

• PreintegrationParams: Parameters for IMU preintegration.

• PreintegratedRotation: Handles gyroscope measurements to track rotation changes.

• AHRSFactor: Attitude and Heading Reference System factor for orientation estimation.

• AttitudeFactor: Factors for attitude estimation from reference directions.

IMU Preintegration (See also below)¶

• PreintegrationBase: Base class for IMU preintegration classes.

• ManifoldPreintegration: Implements IMU preintegration using manifold-based methods as in the Forster et al paper.

• TangentPreintegration: Implements IMU preintegration using tangent space methods, developed at Skydio.

• LieGroupPreintegration: Integrates IMU increments with the NavState $SE_2(3)$ group exponential described by Brossard, Barrau, and Bonnabel.

• ImuFactor: IMU factor.

• CombinedImuFactor: IMU factor with built-in bias evolution.

GNSS Integration¶

• GPSFactor: Factor for incorporating GPS position measurements.

• BarometricFactor: Incorporates barometric altitude measurements.

• PseudorangeFactor: Precise GNSS positioning.

• DopplerFactor: GNSS range-rate measurements for velocity and clock-drift estimation.

Magnetic Field Integration¶

• MagFactor: Factor for incorporating magnetic field measurements.

• MagPoseFactor: Factor for incorporating magnetic field measurements with pose constraints.

Simulation Tools¶

• Scenario: Base class for defining motion scenarios.

• ConstantTwistScenario: Implements constant twist (angular and linear velocity) motion.

• AcceleratingScenario: Implements constantly accelerating motion.

• ScenarioRunner: Executes scenarios and generates IMU measurements.

AHRSFactor and Preintegration¶

This section describes the classes primarily involved in Attitude and Heading Reference Systems (AHRS), which rely on gyroscope measurements for orientation preintegration.

The key components are:

1. Parameters (PreintegratedRotationParams):

   • Stores parameters specific to gyroscope integration, including gyro noise covariance, optional Coriolis terms, and the sensor’s pose relative to the body frame.

2. Rotation Preintegration (PreintegratedRotation):

   • Handles the core logic for integrating gyroscope measurements over time to estimate the change in orientation (deltaRij).

   • Calculates the Jacobian of this integrated rotation with respect to gyroscope bias (delRdelBiasOmega).

3. AHRS Preintegrated Measurements (PreintegratedAhrsMeasurements):

   • Inherits from PreintegratedRotation and adds the calculation and storage of the covariance matrix (preintMeasCov_) associated with the preintegrated rotation.

   • This class specifically accumulates the information needed by the AHRSFactor.

4. AHRS Factor (AHRSFactor):

   • A factor that constrains two Rot3 orientation variables and a Vector3 bias variable using the information accumulated in a PreintegratedAhrsMeasurements object.

   • It effectively measures the consistency between the orientation change predicted by the integrated gyro measurements and the orientation change implied by the factor’s connected state variables.

IMU Factor and Preintegration¶

This section describes the classes involved in preintegrating full IMU measurements (accelerometer and gyroscope) for use in factors like ImuFactor and CombinedImuFactor.

The key components are:

1. Parameters (...Params):

   • PreintegratedRotationParams: Base parameter class (gyro noise, Coriolis, sensor pose).

   • PreintegrationParams: Adds accelerometer noise, gravity vector, integration noise.

   • PreintegrationCombinedParams: Adds parameters for bias random walk covariance.

2. Preintegration Interface (PreintegrationBase):

   • An abstract base class defining the common interface for different IMU preintegration methods. It manages the bias estimate used during integration (biasHat_) and the time interval (deltaTij_).

   • Defines pure virtual methods for integration, bias correction, and state access.

3. Preintegration Implementations:

   • ManifoldPreintegration: Concrete implementation of PreintegrationBase. Integrates directly on the NavState manifold, storing the result as a NavState. Corresponds to Forster et al. RSS 2015.

   • TangentPreintegration: Concrete implementation of PreintegrationBase. Integrates increments in the 9D tangent space of NavState, storing the result as a Vector9.

   • LieGroupPreintegration: Specializes the manifold implementation by applying increments with the NavState $SE_2(3)$ exponential and by using the corresponding nonlinear group bias correction. It stores the result as a NavState.

4. Preintegrated Measurements Containers:

   • PreintegratedImuMeasurements: Stores the result of standard IMU preintegration along with its 9x9 covariance (preintMeasCov_).

   • PreintegratedCombinedMeasurements: Similar, but designed for the CombinedImuFactor. Stores the larger 15x15 covariance matrix (preintMeasCov_) that includes correlations with the bias terms.

5. IMU Factors (...Factor):

   • ImuFactor: A 5-way factor connecting previous pose/velocity, current pose/velocity, and a single (constant during the interval) bias estimate. Does not model bias evolution between factors.

   • ImuFactor2: A 3-way factor connecting previous NavState, current NavState, and a single bias estimate. Functionally similar to ImuFactor but uses the combined NavState type.

   • CombinedImuFactor: A 6-way factor connecting previous pose/velocity, current pose/velocity, previous bias, and current bias. Includes a model for bias random walk evolution between the two bias states.

Important notes¶

• The compiled DefaultPreintegrationType used by ImuFactors and Preintegrated*Measurements is selected by two CMake options:

  • GTSAM_LIEGROUP_PREINTEGRATION=ON selects LieGroupPreintegration and takes precedence if both options are enabled.

  • Otherwise, GTSAM_TANGENT_PREINTEGRATION=ON (the default) selects TangentPreintegration.

  • With both options disabled, ManifoldPreintegration is used. Select this backend for the implementation from Forster et al. (2017).

• If you wish to use any preintegration type other than the default, you must template your PIMs and factors on the desired preintegration type using the template-supporting classes PreintegratedImuMeasurementsT, ImuFactorT, ImuFactor2T, PreintegratedCombinedMeasurementsT, or CombinedImuFactorT.

• Named backend selection is a C++ template API. Python and MATLAB expose only the aliases selected when GTSAM is compiled.

• NavState stores tangent blocks in (R,p,v) order, whereas Brossard et al. write the $SE_2(3)$ matrix in (R,v,p) order. Translate the position and velocity blocks when comparing equations.

• Lie-group integration does not change the chart used by optimization. LieGroupPreintegration applies IMU increments with NavState::expmap, while NavState::retract and localCoordinates retain GTSAM’s component-wise optimization chart.

• omegaCoriolis is the angular velocity of the navigation frame, expressed in navigation-frame coordinates in radians per second. When it is set, PreintegrationBase::predict and AHRS prediction use the exact rotating-Earth transition from Brossard, Barrau, and Bonnabel rather than an additive Coriolis approximation. The covariance recursion is unchanged because Earth rotation changes prediction and residual assembly, not the preintegrated IMU measurement.

• For $w=-\Omega\Delta t$, GTSAM evaluates the exact transition with stable SO(3) kernels. In GTSAM’s kernel conventions, Brossard’s position term is $\Gamma^p=(J_l(w)-\Gamma_{2,l}(w))g\Delta t^2$; using DexpFunctor::Gamma() alone would have the wrong small-angle coefficients. AHRS uses the corresponding exact attitude law $R_j=\operatorname{Exp}(w)R_i\Delta R_{ij}$.

• use2ndOrderCoriolis remains in PreintegrationParams and its serialized layout for compatibility, but it is ignored: the exact model is used for every nonzero omegaCoriolis. NavState::coriolis and PreintegratedRotation::integrateCoriolis are retained as deprecated legacy helpers and are no longer used internally.

• n_gravity must be the gravity vector consistent with the chosen navigation-frame origin. If a local frame is translated, absorb the constant centrifugal contribution associated with that origin shift into n_gravity.

• Using the combined IMU factor is not recommended. Typically biases evolve slowly, and hence a separate, lower frequency Markov chain on the bias is more appropriate.

• For short-duration experiments it is even recommended to use a single constant bias. Bias estimation is notoriously hard to tune/debug, and also acts as a “sink” for any modeling errors. Hence, starting with a constant bias is a good idea to get the rest of the pipeline working.