12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2082 from borglab/feature/ahrs_docs 

AHRS docs and wrap
release/4.3a0
Frank Dellaert 2025-04-06 15:30:05 -04:00 committed by GitHub
parent f4a79517c1 eb705c6a70
commit d04959279e
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 9441 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/AHRSFactor.h
View File

@ -29,7 +29,7 @@
namespace gtsam {
/**
* PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope
* PreintegratedAHRSMeasurements accumulates (integrates) the gyroscope
* measurements (rotation rates) and the corresponding covariance matrix.
* Can be built incrementally so as to avoid costly integration at time of factor construction.
*/
@ -49,7 +49,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
/**
* Default constructor, initialize with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param bias Current estimate of rotation rate biases
*/
PreintegratedAhrsMeasurements(const std::shared_ptr<Params>& p,
const Vector3& biasHat) :
@ -60,7 +60,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
/**
* Non-Default constructor, initialize with measurements
* @param p: Parameters for AHRS pre-integration
* @param bias_hat: Current estimate of acceleration and rotation rate biases
* @param bias_hat: Current estimate of rotation rate biases
* @param deltaTij: Delta time in pre-integration
* @param deltaRij: Delta rotation in pre-integration
* @param delRdelBiasOmega: Jacobian of rotation wrt. to gyro bias
@ -87,11 +87,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
/// equals
bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
/// Reset inetgrated quantities to zero
/// Reset integrated quantities to zero
void resetIntegration();
/**
* Add a single Gyroscope measurement to the preintegration.
* Add a single gyroscope measurement to the preintegration.
* Measurements are taken to be in the sensor
* frame and conversion to the body frame is handled by `body_P_sensor` in
* `PreintegratedRotationParams` (if provided).

2
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
void PreintegratedRotationParams::print(const string& s) const {
cout << (s.empty() ? s : s + "\n") << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
cout << "gyroscopeCovariance:\n" << gyroscopeCovariance << endl;
if (omegaCoriolis)
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
if (body_P_sensor) body_P_sensor->print("body_P_sensor");

14
gtsam/navigation/PreintegratedRotation.h
View File

@ -126,12 +126,12 @@ class GTSAM_EXPORT PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
/// Default constructor for serialization
PreintegratedRotation() {}
public:
public:
/// @name Constructors
/// @{
/// Default constructor for serialization
PreintegratedRotation() {}
/// Default constructor, resets integration to zero
explicit PreintegratedRotation(const std::shared_ptr<Params>& p) : p_(p) {
@ -149,9 +149,6 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/// check parameters equality: checks whether shared pointer points to same Params object.
bool matchesParamsWith(const PreintegratedRotation& other) const {
return p_ == other.p_;
@ -175,6 +172,9 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @name Main functionality
/// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/**
* @brief Calculate an incremental rotation given the gyro measurement and a
* time interval, and update both deltaTij_ and deltaRij_.

215
gtsam/navigation/doc/AHRSFactor.ipynb Normal file
View File

@ -0,0 +1,215 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# AHRSFactor\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/AHRSFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\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",
"\n",
"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": {},
"source": [
"## Mathematical Formulation\n",
"\n",
"The `AHRSFactor` relies on the use of `PreintegratedRotation`, which applies successive exponential maps to each individual gyroscope measurement $\\omega$ over the interval $[t_i, t_j]$. In this approach, every measurement contributes a small rotation given by $\\exp(\\omega_k \\Delta t)$, and the overall preintegrated rotation is obtained by composing these incremental rotations:\n",
"$$\n",
"\\Delta R_{ij}^{meas} = \\prod_{k} \\exp(\\omega_k \\Delta t)\n",
"$$\n",
"\n",
"Given two estimated rotations $R_i$ at time $t_i$ and $R_j$ at time $t_j$, the factor enforces:\n",
"$$\n",
"R_j \\approx R_i \\cdot \\Delta R_{ij}^{meas}\n",
"$$\n",
"\n",
"The error term optimized by the factor graph is the rotational discrepancy captured by the logarithmic map:\n",
"$$\n",
"e = \\text{Log}\\left((\\Delta R_{ij}^{meas})^T \\cdot R_i^T R_j\\right)\n",
"$$\n",
"\n",
"Note: the reality is a bit more complicated, because the code also takes into account the effects of bias, and if desired, coriolis forces."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality\n",
"\n",
"### Constructor\n",
"\n",
"The constructor initializes the factor using a preintegrated AHRS measurement object, relating orientation states at discrete time instances $t_i$ and $t_j$.\n",
"\n",
"### Core Methods\n",
"\n",
"- `evaluateError`: calculates the error between two estimated orientations at times $t_i$ and $t_j$, relative to the preintegrated gyroscopic measurements. The error is computed as:\n",
"\n",
" $$\n",
" \\text{error} = \\text{Log}\\left((\\Delta R_{ij}^{meas})^T \\cdot R_i^T R_j\\right)\n",
" $$\n",
"\n",
" Here:\n",
"\n",
" - $R_i, R_j$ are the estimated rotation matrices at times $t_i$ and $t_j$.\n",
" - $\\Delta R_{ij}^{meas}$ is the rotation matrix obtained by integrating gyroscope measurements from $t_i$ to $t_j$.\n",
" - $\\text{Log}(\\cdot)$ is the logarithmic map from $SO(3)$ to $\\mathbb{R}^3$.\n",
"\n",
" The resulting 3-dimensional error vector represents the rotational discrepancy.\n",
"\n",
"- `print`: outputs a readable representation of the internal state of the factor, including the associated time steps and preintegrated measurements, aiding debugging and verification.\n",
"\n",
"- `equals` determines if another `AHRSFactor` instance is identical, useful in testing scenarios or when verifying the correctness of factor graph constructions."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage\n",
"\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",
"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": {
"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,
"nbformat_minor": 2
}

9073
gtsam/navigation/doc/PreintegratedRotation.ipynb Normal file
View File

File diff suppressed because it is too large Load Diff

70
gtsam/navigation/doc/PreintegrationParams.ipynb Normal file
View File

@ -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
}

35
gtsam/navigation/navigation.i
View File

@ -97,6 +97,29 @@ virtual class PreintegratedRotationParams {
void serialize() const;
};
class PreintegratedRotation {
// Constructors
PreintegratedRotation(const gtsam::PreintegratedRotationParams* params);
// Standard Interface
void resetIntegration();
void integrateGyroMeasurement(const gtsam::Vector& measuredOmega, const gtsam::Vector& biasHat, double deltaT);
gtsam::Rot3 biascorrectedDeltaRij(const gtsam::Vector& biasOmegaIncr) const;
gtsam::Vector integrateCoriolis(const gtsam::Rot3& rot_i) const;
// Access instance variables
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
gtsam::Matrix delRdelBiasOmega() const;
// Testable
void print(string s = "") const;
bool equals(const gtsam::PreintegratedRotation& expected, double tol) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(gtsam::Vector n_gravity);
@ -291,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;
@ -306,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 <gtsam/navigation/AttitudeFactor.h>

36
gtsam/navigation/navigation.md Normal file
View File

@ -0,0 +1,36 @@
# Navigation
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
- **[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
- **[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
- **[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
- **[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
- **[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.
These components together provide a comprehensive framework for fusing inertial data with other sensor measurements in navigation and robotics applications.

3
myst.yml
View File

@ -13,6 +13,9 @@ project:
- file: ./gtsam/geometry/geometry.md
children:
- pattern: ./gtsam/geometry/doc/*
- file: ./gtsam/navigation/navigation.md
children:
- pattern: ./gtsam/navigation/doc/*
site:
nav:
- title: Getting started