Split AHRS and IMU overviews
							parent
							
								
									b29ca85739
								
							
						
					
					
						commit
						12d74ad50c
					
				|  | @ -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 { | ||||
|      <<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 | ||||
| ``` | ||||
		Loading…
	
		Reference in New Issue