diff --git a/gtsam/navigation/navigation.md b/gtsam/navigation/navigation.md index 8f75e5100..706e8d7a3 100644 --- a/gtsam/navigation/navigation.md +++ b/gtsam/navigation/navigation.md @@ -33,4 +33,101 @@ The `navigation` module in GTSAM provides specialized tools for inertial navigat - **[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. -These components together provide a comprehensive framework for fusing inertial data with other sensor measurements in navigation and robotics applications. \ No newline at end of file +# IMU Factor and Preintegration Overview + +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) + +```mermaid +classDiagram + direction TD + + 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_ + +Rot3 deltaRij_ + +Matrix3 delRdelBiasOmega_ + +integrateGyroMeasurement() + +biascorrectedDeltaRij() + } + PreintegratedRotation ..> PreintegratedRotationParams : uses + + class PreintegrationBase { + <> + +imuBias::ConstantBias biasHat_ + +resetIntegration()* + +integrateMeasurement()* + +biasCorrectedDelta()* + +predict() + +computeError() + } + PreintegrationBase --|> PreintegratedRotation : inherits + + class ManifoldPreintegration { + +NavState deltaXij_ + +update() + } + ManifoldPreintegration --|> PreintegrationBase : implements + + 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_ + } + + PreintegratedCombinedMeasurements --|> TangentPreintegration : inherits + class PreintegratedCombinedMeasurements { + +Matrix preintMeasCov_ (15x15) + } + PreintegratedCombinedMeasurements ..> PreintegrationCombinedParams : uses + + class ImuFactor { + } + ImuFactor ..> PreintegratedImuMeasurements : uses + + class ImuFactor2 { + } + ImuFactor2 ..> PreintegratedImuMeasurements : uses + + class CombinedImuFactor { + } + CombinedImuFactor ..> PreintegratedCombinedMeasurements : uses + + class AHRSFactor { + } + AHRSFactor ..> PreintegratedAhrsMeasurements : uses +``` \ No newline at end of file