Split AHRS and IMU overviews

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.
+This section describes the classes primarily involved in Attitude and Heading Reference Systems (AHRS), which rely on gyroscope measurements for orientation preintegration.
-
-## Class Relationship Diagram (Mermaid)
 
 ```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 {
+     <<Parameter Base>>
+    }
+    class PreintegrationParams {
+     <<Parameters>>
+     +Matrix3 accelerometerCovariance
+     +Vector3 n_gravity
+    }
+    PreintegrationParams --|> PreintegratedRotationParams : inherits
+
+    class PreintegrationCombinedParams {
+     <<Parameters>>
+     +Matrix3 biasAccCovariance
+     +Matrix3 biasOmegaCovariance
+    }
+    PreintegrationCombinedParams --|> PreintegrationParams : inherits
+
     class PreintegrationBase {
         <<Abstract>>
         +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
-```