diff --git a/gtsam/navigation/doc/ImuFactor.ipynb b/gtsam/navigation/doc/ImuFactor.ipynb index 7e32c0354..0d0afa526 100644 --- a/gtsam/navigation/doc/ImuFactor.ipynb +++ b/gtsam/navigation/doc/ImuFactor.ipynb @@ -10,6 +10,8 @@ "\n", "## Overview\n", "\n", + "### ImuFactor (5-way Factor)\n", + "\n", "The `ImuFactor` is the standard GTSAM factor for incorporating preintegrated IMU measurements into a factor graph. It's a 5-way factor connecting:\n", "\n", "1. Pose at time $i$ (`Pose3`)\n", @@ -20,7 +22,21 @@ "\n", "It takes a `PreintegratedImuMeasurements` object, which summarizes the IMU readings between times $i$ and $j$. The factor's error function measures the discrepancy between the relative motion predicted by the preintegrated measurements (corrected for the *current* estimate of the bias at time $i$) and the relative motion implied by the state variables ($Pose_i, Vel_i, Pose_j, Vel_j$) connected to the factor.\n", "\n", - "**Important Note:** This factor assumes the bias is *constant* between time $i$ and $j$ for the purpose of evaluating its error. It does *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead." + "### ImuFactor2: NavState Variant\n", + "\n", + "The `ImuFactor2` is ternary variant of the `ImuFactor` that operates directly on `NavState` objects instead of separate `Pose3` and `Vector3` variables for pose and velocity. This simplifies the factor graph by reducing the number of connected variables and can make the graph more efficient to optimize.\n", + "\n", + "Instead of connecting five variables (`Pose_i`, `Vel_i`, `Pose_j`, `Vel_j`, `Bias_i`), the `ImuFactor2` connects three:\n", + "\n", + "1. `NavState` at time $i$ (`NavState` combines pose and velocity)\n", + "2. `NavState` at time $j`\n", + "3. IMU Bias at time $i$ (`imuBias::ConstantBias`)\n", + "\n", + "### Modeling Bias\n", + "\n", + "Both factors assume that the bias is *constant* between time $i$ and $j$ for the purpose of evaluating its error. That is typically a very good assumption, as bias evolves slowly over time.\n", + "\n", + "The factors do *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead." ] }, { @@ -94,7 +110,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 2, "metadata": {}, "outputs": [ { @@ -188,6 +204,77 @@ "print(\"Factor error (0.5 * ||error||^2_Sigma):\", graph.error(values))" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can also use `ImuFactor2`, with `NavState`, giving exactly the same result:" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Created ImuFactor2:\n", + "ImuFactor2(x0,x1,b0)\n", + "preintegrated measurements:\n", + "\n", + " deltaTij = 0.1\n", + " deltaRij.ypr = ( 0 -0 0)\n", + " deltaPij = 0 0 -0.04905\n", + " deltaVij = 0 0 -0.981\n", + " gyrobias = 0 0 0\n", + " acc_bias = 0 0 0\n", + "\n", + " preintMeasCov \n", + "[ 1e-05 0 0 0 1.39793e-07 0 0 4.4145e-06 0\n", + " 0 1e-05 0 -1.39793e-07 0 0 -4.4145e-06 0 0\n", + " 0 0 1e-05 0 0 0 0 0 0\n", + " 0 -1.39793e-07 0 3.32969e-06 0 0 5.00974e-05 0 0\n", + " 1.39793e-07 0 0 0 3.32969e-06 0 0 5.00974e-05 0\n", + " 0 0 0 0 0 3.326e-06 0 0 5e-05\n", + " 0 -4.4145e-06 0 5.00974e-05 0 0 0.00100274 0 0\n", + " 4.4145e-06 0 0 0 5.00974e-05 0 0 0.00100274 0\n", + " 0 0 0 0 0 5e-05 0 0 0.001]\n", + " noise model sigmas: 0.00316228 0.00316228 0.00316228 0.00182474 0.00182474 0.00182373 0.0316661 0.0316661 0.0316228\n", + "\n", + "Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]\n", + "Factor error (0.5 * ||error||^2_Sigma): 0.0\n" + ] + } + ], + "source": [ + "from gtsam import ImuFactor2\n", + "\n", + "# 1. Create the ImuFactor2\n", + "# The noise model is automatically derived from pim.preintMeasCov()\n", + "imu_factor2 = ImuFactor2(X(0), X(1), B(0), pim)\n", + "\n", + "print(\"Created ImuFactor2:\")\n", + "imu_factor2.print()\n", + "\n", + "# 2. Example: Evaluate error with perfect states (should be near zero)\n", + "graph = NonlinearFactorGraph()\n", + "graph.add(imu_factor2)\n", + "\n", + "values = Values()\n", + "nav_state_i = NavState(pose_i, vel_i)\n", + "nav_state_j = pim.predict(nav_state_i, bias_i) # Use bias_i=bias_hat\n", + "\n", + "values.insert(X(0), nav_state_i)\n", + "values.insert(X(1), nav_state_j)\n", + "values.insert(B(0), bias_i)\n", + "\n", + "error_vector = imu_factor2.evaluateError(nav_state_i, nav_state_j, bias_i)\n", + "print(\"\\nError vector (should be near zero):\", error_vector)\n", + "print(\"Factor error (0.5 * ||error||^2_Sigma):\", graph.error(values))" + ] + }, { "cell_type": "markdown", "metadata": {}, diff --git a/gtsam/navigation/navigation.md b/gtsam/navigation/navigation.md index 8d08a5a6e..704b8c7ad 100644 --- a/gtsam/navigation/navigation.md +++ b/gtsam/navigation/navigation.md @@ -10,29 +10,35 @@ The `navigation` module in GTSAM provides specialized tools for inertial navigat ### 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. +- **[PreintegrationParams](doc/PreintegrationParams.ipynb)**: Parameters for IMU preintegration. +- **[PreintegratedRotation](doc/PreintegratedRotation.ipynb)**: Handles gyroscope measurements to track rotation changes. +- **[AHRSFactor](doc/AHRSFactor.ipynb)**: Attitude and Heading Reference System factor for orientation estimation. +- **[AttitudeFactor](doc/AttitudeFactor.ipynb)**: Factors for attitude estimation from reference directions. ### 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. +- **[ImuFactor](doc/ImuFactor.ipynb)**: IMU factor. +- **[CombinedImuFactor](doc/CombinedImuFactor.ipynb)**: IMU factor with built-in bias evolution. ### 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. +- **[GPSFactor](doc/GPSFactor.ipynb)**: Factor for incorporating GPS position measurements. +- **[BarometricFactor](doc/BarometricFactor.ipynb)**: Incorporates barometric altitude measurements. + +### Magnetic Field Integration + +- **[MagFactor](doc/MagFactor.ipynb)**: Factor for incorporating magnetic field measurements. +- **[MagPoseFactor](doc/MagPoseFactor.ipynb)**: Factor for incorporating magnetic field measurements with pose constraints. ### Simulation Tools -- **[Scenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Base class for defining motion scenarios. +- **[Scenario](doc/Scenario.ipynb)**: 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. +- **[ScenarioRunner](doc/ScenarioRunner.ipynb)**: Executes scenarios and generates IMU measurements. ## AHRSFactor and Preintegration @@ -73,7 +79,7 @@ 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`)**: +2. **Rotation Preintegration ([PreintegratedRotation](doc/PreintegratedRotation.ipynb))**: * 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`). @@ -81,7 +87,7 @@ The key components are: * 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`)**: +4. **AHRS Factor ([AHRSFactor](doc/AHRSFactor.ipynb))**: * 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. @@ -189,9 +195,9 @@ The key components are: * `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. + * [ImuFactor](doc/ImuFactor.ipynb): 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](doc/ImuFactor.ipynb): 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](doc/CombinedImuFactor.ipynb): 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.