From 12d74ad50c4133d110b495898a6c54a20fbcf1c8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 6 Apr 2025 23:49:35 -0400 Subject: [PATCH] Split AHRS and IMU overviews --- gtsam/navigation/navigation.md | 151 ++++++++++++++++++++++++--------- 1 file changed, 110 insertions(+), 41 deletions(-) diff --git a/gtsam/navigation/navigation.md b/gtsam/navigation/navigation.md index 706e8d7a3..f7797b972 100644 --- a/gtsam/navigation/navigation.md +++ b/gtsam/navigation/navigation.md @@ -2,66 +2,51 @@ 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: -## Core Navigation Types +## Classes +### Core Navigation Types - **[NavState](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/NavState.h)**: Represents the complete navigation state $\mathcal{SE}_2(3)$, i.e., attitude, position, and velocity. It also implements the group ${SE}_2(3)$. - **[ImuBias](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuBias.h)**: Models constant biases in IMU measurements (accelerometer and gyroscope). -## Attitude Estimation +### Attitude Estimation - **[PreintegrationParams](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.h)**: Parameters for IMU preintegration. - **[PreintegratedRotation](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegratedRotation.h)**: Handles gyroscope measurements to track rotation changes. - **[AHRSFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.h)**: Attitude and Heading Reference System factor for orientation estimation. - **[AttitudeFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.h)**: Factors for attitude estimation from reference directions. -## IMU Preintegration +### IMU Preintegration (See also below) - **[PreintegrationBase](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationBase.h)**: Base class for IMU preintegration classes. - **[ManifoldPreintegration](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldPreintegration.h)**: Implements IMU preintegration using manifold-based methods as in the Forster et al paper. - **[TangentPreintegration](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/TangentPreintegration.h)**: Implements IMU preintegration using tangent space methods, developed at Skydio. - **[CombinedImuFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.h)**: Improved IMU factor with bias evolution. -## GNSS Integration +### GNSS Integration - **[GPSFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.h)**: Factor for incorporating GPS position measurements. - **[BarometricFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.h)**: Incorporates barometric altitude measurements. -## Simulation Tools +### Simulation Tools - **[Scenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Base class for defining motion scenarios. - **[ConstantTwistScenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Implements constant twist (angular and linear velocity) motion. - **[AcceleratingScenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Implements constantly accelerating motion. - **[ScenarioRunner](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ScenarioRunner.h)**: Executes scenarios and generates IMU measurements. -# IMU Factor and Preintegration Overview +## AHRSFactor and Preintegration -This document provides an overview of the key classes involved in IMU preintegration and factor creation within the `gtsam/navigation` module. These components are essential for performing state estimation using Inertial Measurement Unit data, often fused with other sensors like cameras or GPS. - -## Class Relationship Diagram (Mermaid) +This section describes the classes primarily involved in Attitude and Heading Reference Systems (AHRS), which rely on gyroscope measurements for orientation preintegration. ```mermaid classDiagram - direction TD + direction LR class PreintegratedRotationParams { +Matrix3 gyroscopeCovariance +Vector3 omegaCoriolis +Pose3 body_P_sensor } - class PreintegrationParams { - +Matrix3 accelerometerCovariance - +Matrix3 integrationCovariance - +bool use2ndOrderCoriolis - +Vector3 n_gravity - } - PreintegrationParams --|> PreintegratedRotationParams : inherits - - class PreintegrationCombinedParams { - +Matrix3 biasAccCovariance - +Matrix3 biasOmegaCovariance - +Matrix6 biasAccOmegaInt - } - PreintegrationCombinedParams --|> PreintegrationParams : inherits class PreintegratedRotation { +double deltaTij_ @@ -72,16 +57,70 @@ classDiagram } PreintegratedRotation ..> PreintegratedRotationParams : uses + class PreintegratedAhrsMeasurements { + +Matrix3 preintMeasCov_ + } + PreintegratedAhrsMeasurements --|> PreintegratedRotation : inherits + + class AHRSFactor { + +evaluateError(...) Vector3 + } + AHRSFactor ..> PreintegratedAhrsMeasurements : uses +``` + +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`. + +```mermaid +classDiagram + direction TD + + class PreintegratedRotationParams { + <> + } + class PreintegrationParams { + <> + +Matrix3 accelerometerCovariance + +Vector3 n_gravity + } + PreintegrationParams --|> PreintegratedRotationParams : inherits + + class PreintegrationCombinedParams { + <> + +Matrix3 biasAccCovariance + +Matrix3 biasOmegaCovariance + } + PreintegrationCombinedParams --|> PreintegrationParams : inherits + class PreintegrationBase { <> +imuBias::ConstantBias biasHat_ + +double deltaTij_ +resetIntegration()* +integrateMeasurement()* +biasCorrectedDelta()* +predict() +computeError() } - PreintegrationBase --|> PreintegratedRotation : inherits + PreintegrationBase ..> PreintegrationParams : uses class ManifoldPreintegration { +NavState deltaXij_ @@ -91,43 +130,73 @@ classDiagram class TangentPreintegration { +Vector9 preintegrated_ - +Matrix93 preintegrated_H_biasAcc_ - +Matrix93 preintegrated_H_biasOmega_ +update() } TangentPreintegration --|> PreintegrationBase : implements - class PreintegratedAhrsMeasurements { - +Matrix3 preintMeasCov_ - } - PreintegratedAhrsMeasurements --|> PreintegratedRotation : inherits - - %% Assuming PreintegrationType is TangentPreintegration for example %% - %% Change TangentPreintegration to ManifoldPreintegration if needed %% - PreintegratedImuMeasurements --|> TangentPreintegration : inherits class PreintegratedImuMeasurements { +Matrix9 preintMeasCov_ } + PreintegratedImuMeasurements --|> PreintegrationType : inherits - PreintegratedCombinedMeasurements --|> TangentPreintegration : inherits class PreintegratedCombinedMeasurements { +Matrix preintMeasCov_ (15x15) } + PreintegratedCombinedMeasurements --|> PreintegrationType : inherits PreintegratedCombinedMeasurements ..> PreintegrationCombinedParams : uses + class PreintegrationType{ + } + PreintegrationType --|> ManifoldPreintegration : typedef + PreintegrationType --|> TangentPreintegration : typedef + class ImuFactor { + Pose3, Vector3, Pose3, Vector3, ConstantBias + +evaluateError(...) Vector9 } ImuFactor ..> PreintegratedImuMeasurements : uses - class ImuFactor2 { + NavState, NavState, ConstantBias + +evaluateError(...) Vector9 } ImuFactor2 ..> PreintegratedImuMeasurements : uses + class CombinedImuFactor { + Pose3, Vector3, Pose3, Vector3, ConstantBias + +evaluateError(...) Vector (15) } CombinedImuFactor ..> PreintegratedCombinedMeasurements : uses +``` + +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`. + +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 +- Which implementation is used for `PreintegrationType` depends on the compile flag `GTSAM_TANGENT_PREINTEGRATION`, which is true by default. + - If false, `ManifoldPreintegration` is used. Please use this setting to get the exact implementation from {cite:t}`https://doi.org/10.1109/TRO.2016.2597321`. + - If true, `TangentPreintegration` is used. This does the integration on the tangent space of the NavState manifold. +- 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. - class AHRSFactor { - } - AHRSFactor ..> PreintegratedAhrsMeasurements : uses -``` \ No newline at end of file