diff --git a/gtsam/navigation/doc/AHRSFactor.ipynb b/gtsam/navigation/doc/AHRSFactor.ipynb index 6de979e17..452ca13c2 100644 --- a/gtsam/navigation/doc/AHRSFactor.ipynb +++ b/gtsam/navigation/doc/AHRSFactor.ipynb @@ -6,6 +6,8 @@ "source": [ "# AHRSFactor\n", "\n", + "\"Open\n", + "\n", "## Overview\n", "\n", "The `AHRSFactor` class implements a factor to implement an *Attitude and Heading Reference System* (AHRS) within GTSAM. It is a binary factor, taking preintegrated measurements from a gyroscope between two discrete time steps, typically denoted as $t_i$ and $t_j$. These preintegrated measurements encapsulate the rotational motion observed by an inertial measurement unit (IMU) between these two timestamps.\n", @@ -13,6 +15,27 @@ "The `AHRSFactor` thus constrains two attitude states (represented as elements of $SO(3)$) based solely on gyroscope measurements. Accelerometer or magnetometer aiding, needed to build a complete AHRS system, must be handled separately." ] }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Note: you may need to restart the kernel to use updated packages.\n" + ] + } + ], + "source": [ + "%pip install --quiet gtsam plotly" + ] + }, { "cell_type": "markdown", "metadata": {}, @@ -72,19 +95,119 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "## Typical Use Case\n", + "## Usage\n", "\n", - "`AHRSFactor` is typically applied within factor-graph-based orientation estimators where orientation states are discretized in time. Its primary role is to incorporate gyroscope information in a computationally efficient and numerically stable way, especially useful in IMU-centric navigation scenarios like:\n", + "First, create a PreintegratedAhrsMeasurements (PAM) object by supplying the necessary IMU parameters. " + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "from gtsam import PreintegrationParams, PreintegratedAhrsMeasurements\n", "\n", - "- Robotics and autonomous vehicles\n", - "- UAV navigation systems\n", - "- Motion capture and tracking systems" + "params = PreintegrationParams.MakeSharedU(9.81)\n", + "params.setGyroscopeCovariance(np.deg2rad(1)*np.eye(3))\n", + "params.setAccelerometerCovariance(0.01*np.eye(3))\n", + "\n", + "biasHat = np.zeros(3) # Assuming no bias for simplicity\n", + "pam = PreintegratedAhrsMeasurements(params, biasHat) " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\n", + "Next, integrate each gyroscope measurement along with its corresponding time interval to accumulate the preintegrated rotation." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import Point3\n", + "np.random.seed(42) # For reproducibility\n", + "for _ in range(15): # Add 15 random measurements, biased to move around z-axis\n", + " omega = Point3(0,0,-0.5) + 0.1*np.random.randn(3) # Random angular velocity vector\n", + " pam.integrateMeasurement(omega, deltaT=0.1)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, construct the AHRSFactor using the accumulated PAM and the keys representing the rotation states at the two time instants." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import AHRSFactor\n", + "bias_key = 0 # Key for the bias\n", + "i, j = 1, 2 # Indices of the two attitude unknowns\n", + "ahrs_factor = AHRSFactor(i, j, bias_key, pam)" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "AHRSFactor(1,2,0, preintegrated measurements: deltaTij [1.5]\n", + " deltaRij.ypr = ( -0.82321 -0.0142842 0.0228577)\n", + "biasHat [0 0 0]\n", + " PreintMeasCov [ 0.0261799 1.73472e-18 1.35525e-20\n", + "1.73472e-18 0.0261799 9.23266e-20\n", + "1.35525e-20 1.17738e-19 0.0261799 ]\n", + " noise model: diagonal sigmas [0.161802159; 0.161802159; 0.161802159];\n", + "\n" + ] + } + ], + "source": [ + "print(ahrs_factor)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Source\n", + "- [AHRSFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.h)\n", + "- [AHRSFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.cpp)" ] } ], "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, "language_info": { - "name": "python" + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/navigation/doc/PreintegratedRotation.ipynb b/gtsam/navigation/doc/PreintegratedRotation.ipynb index 56349f32a..40392cfdd 100644 --- a/gtsam/navigation/doc/PreintegratedRotation.ipynb +++ b/gtsam/navigation/doc/PreintegratedRotation.ipynb @@ -21,7 +21,11 @@ { "cell_type": "code", "execution_count": 1, - "metadata": {}, + "metadata": { + "tags": [ + "remove-cell" + ] + }, "outputs": [ { "name": "stdout", @@ -118,6 +122,23 @@ "print(params)" ] }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "You can (and should) configure using your gyroscope's noise covariance matrix and the optional Coriolis acceleration compensation vector:\n", + "\n", + "### Setters\n", + "- `setGyroscopeCovariance`: Sets the 3×3 continuous-time noise covariance matrix for gyroscope measurements.\n", + "- `setOmegaCoriolis`: Sets an optional Coriolis acceleration compensation vector.\n", + "- `setBodyPSensor`: Sets an optional pose transformation between the body frame and the sensor frame.\n", + "\n", + "### Getters\n", + "- `getGyroscopeCovariance`: Returns the gyroscope covariance matrix.\n", + "- `getOmegaCoriolis`: Returns the optional Coriolis acceleration vector.\n", + "- `getBodyPSensor`: Returns the optional body-to-sensor pose transformation." + ] + }, { "cell_type": "markdown", "metadata": {}, @@ -9017,6 +9038,15 @@ "fig.update_layout(scene_camera=dict(eye=dict(x=1.1, y=-1.1, z=0.2)), margin=dict(l=0, r=0, t=0, b=0))\n", "fig.show()" ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Source\n", + "- [PreintegratedRotation.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegratedRotation.h)\n", + "- [PreintegratedRotation.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegratedRotation.cpp)" + ] } ], "metadata": { diff --git a/gtsam/navigation/doc/PreintegrationParams.ipynb b/gtsam/navigation/doc/PreintegrationParams.ipynb new file mode 100644 index 000000000..aaf6480b5 --- /dev/null +++ b/gtsam/navigation/doc/PreintegrationParams.ipynb @@ -0,0 +1,70 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# PreintegrationParams\n", + "\n", + "The `PreintegrationParams` class extends `PreintegratedRotationParams` to provide a complete configuration for IMU preintegration, including accelerometer measurements:\n", + "\n", + "| Parameter | Description |\n", + "|-----------|-------------|\n", + "| `accelerometerCovariance` | 3×3 continuous-time noise covariance matrix for accelerometer measurements (units: m²/s⁴/Hz) |\n", + "| `integrationCovariance` | 3×3 covariance matrix representing additional uncertainty during integration |\n", + "| `use2ndOrderCoriolis` | Boolean flag to enable more accurate Coriolis effect compensation |\n", + "| `n_gravity` | Gravity vector in the navigation frame (units: m/s²) |\n", + "\n", + "## Convenience API\n", + "\n", + "The class provides convenient factory methods for the two most common navigation frame conventions:\n", + "- `MakeSharedD()`: Creates parameters for a Z-down frame (like NED - North-East-Down)\n", + "- `MakeSharedU()`: Creates parameters for a Z-up frame (like ENU - East-North-Up)\n", + "\n", + "Both methods also take a [local gravity constant](https://www.sensorsone.com/local-gravity-calculator/), which decreases as one gets closer to the equator. The default is $9.81m/s^2$." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Detailed API\n", + "\n", + "- The constructor requires the navigation frame gravity vector as an argument.\n", + "\n", + "### Setters\n", + "- `setGyroscopeCovariance`: Sets the 3×3 continuous-time noise covariance matrix for gyroscope measurements.\n", + "- `setOmegaCoriolis`: Sets an optional Coriolis acceleration compensation vector.\n", + "- `setBodyPSensor`: Sets an optional pose transformation between the body frame and the sensor frame.\n", + "- `setAccelerometerCovariance`: Sets the accelerometer covariance matrix.\n", + "- `setIntegrationCovariance`: Sets the integration covariance matrix.\n", + "- `setUse2ndOrderCoriolis`: Sets the flag controlling the use of second order Coriolis compensation.\n", + "\n", + "### Getters\n", + "- `getGyroscopeCovariance`: Returns the gyroscope covariance matrix.\n", + "- `getOmegaCoriolis`: Returns the optional Coriolis acceleration vector.\n", + "- `getBodyPSensor`: Returns the optional body-to-sensor pose transformation.\n", + "- `getAccelerometerCovariance`: Returns the current accelerometer covariance matrix.\n", + "- `getIntegrationCovariance`: Returns the current integration covariance matrix.\n", + "- `getGravity`: Returns the navigation frame gravity vector.\n", + "- `isUsing2ndOrderCoriolis`: Returns the status of the second order Coriolis flag." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Source\n", + "- [PreintegrationParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.h)\n", + "- [PreintegrationParams.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.cpp)" + ] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index f66a38b47..3796153e3 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -314,10 +314,7 @@ class PreintegratedAhrsMeasurements { virtual class AHRSFactor : gtsam::NonlinearFactor { AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements); // Standard Interface gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; @@ -329,6 +326,13 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { // enable serialization functionality void serialize() const; + + // deprecated: + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); }; #include diff --git a/gtsam/navigation/navigation.md b/gtsam/navigation/navigation.md index a220398de..8f75e5100 100644 --- a/gtsam/navigation/navigation.md +++ b/gtsam/navigation/navigation.md @@ -9,13 +9,13 @@ 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. ## IMU Preintegration -- **[PreintegrationParams](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.h)**: Base parameters for IMU preintegration. - **[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.