Add files and links
							parent
							
								
									a68d3ffbd2
								
							
						
					
					
						commit
						5eaba687de
					
				|  | @ -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": {}, | ||||
|  |  | |||
|  | @ -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. | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue