Merge branch 'develop' into gpt-docs
Frank Dellaert
GitHub
commit
2991ce67d7
|
|
@ -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).
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -126,13 +126,13 @@ 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) {
|
||||
resetIntegration();
|
||||
|
|
@ -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_.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
Loading…
Reference in New Issue