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

Merge pull request #2084 from borglab/feature/more_nav_docs 

More navigation docs
release/4.3a0
Frank Dellaert 2025-04-07 18:46:47 -04:00 committed by GitHub
parent c2f9c5e4af 5eaba687de
commit 967c3edfbd
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
22 changed files with 3274 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -41,7 +41,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
~ConstantVelocityFactor() override {}
/**
* @brief Caclulate error: (x2 - x1.update(dt)))
* @brief Calculate error: (x2 - x1.update(dt)))
* where X1 and X1 are NavStates and dt is
* the time difference in seconds between the states.
* @param x1 NavState for key a

32
gtsam/navigation/GPSFactor.cpp
View File

@ -37,9 +37,9 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
}
//***************************************************************************
Vector GPSFactor::evaluateError(const Pose3& p,
Vector GPSFactor::evaluateError(const Pose3& nTb,
OptionalMatrixType H) const {
return p.translation(H) -nT_;
return nTb.translation(H) -nT_;
}
//***************************************************************************
@ -82,9 +82,9 @@ bool GPSFactorArm::equals(const NonlinearFactor& expected, double tol) const {
}
//***************************************************************************
Vector GPSFactorArm::evaluateError(const Pose3& p,
Vector GPSFactorArm::evaluateError(const Pose3& nTb,
OptionalMatrixType H) const {
const Matrix3 nRb = p.rotation().matrix();
const Matrix3 nRb = nTb.rotation().matrix();
if (H) {
H->resize(3, 6);
@ -92,7 +92,7 @@ Vector GPSFactorArm::evaluateError(const Pose3& p,
H->block<3, 3>(0, 3) = nRb;
}
return p.translation() + nRb * bL_ - nT_;
return nTb.translation() + nRb * bL_ - nT_;
}
//***************************************************************************
@ -110,9 +110,9 @@ bool GPSFactorArmCalib::equals(const NonlinearFactor& expected, double tol) cons
}
//***************************************************************************
Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
Vector GPSFactorArmCalib::evaluateError(const Pose3& nTb, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.rotation().matrix();
const Matrix3 nRb = nTb.rotation().matrix();
if (H1) {
H1->resize(3, 6);
@ -124,7 +124,7 @@ Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
*H2 = nRb;
}
return p.translation() + nRb * bL - nT_;
return nTb.translation() + nRb * bL - nT_;
}
//***************************************************************************
@ -142,9 +142,9 @@ bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
}
//***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p,
Vector GPSFactor2::evaluateError(const NavState& nTb,
OptionalMatrixType H) const {
return p.position(H) -nT_;
return nTb.position(H) -nT_;
}
//***************************************************************************
@ -164,9 +164,9 @@ bool GPSFactor2Arm::equals(const NonlinearFactor& expected, double tol) const {
}
//***************************************************************************
Vector GPSFactor2Arm::evaluateError(const NavState& p,
Vector GPSFactor2Arm::evaluateError(const NavState& nTb,
OptionalMatrixType H) const {
const Matrix3 nRb = p.attitude().matrix();
const Matrix3 nRb = nTb.attitude().matrix();
if (H) {
H->resize(3, 9);
@ -175,7 +175,7 @@ Vector GPSFactor2Arm::evaluateError(const NavState& p,
H->block<3, 3>(0, 6).setZero();
}
return p.position() + nRb * bL_ - nT_;
return nTb.position() + nRb * bL_ - nT_;
}
//***************************************************************************
@ -193,9 +193,9 @@ bool GPSFactor2ArmCalib::equals(const NonlinearFactor& expected, double tol) con
}
//***************************************************************************
Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
Vector GPSFactor2ArmCalib::evaluateError(const NavState& nTb, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.attitude().matrix();
const Matrix3 nRb = nTb.attitude().matrix();
if (H1) {
H1->resize(3, 9);
@ -208,7 +208,7 @@ Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
*H2 = nRb;
}
return p.position() + nRb * bL - nT_;
return nTb.position() + nRb * bL - nT_;
}
}/// namespace gtsam

12
gtsam/navigation/GPSFactor.h
View File

@ -83,7 +83,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
@ -171,7 +171,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
@ -242,7 +242,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const Pose3& p, const Point3& bL, OptionalMatrixType H1,
Vector evaluateError(const Pose3& nTb, const Point3& bL, OptionalMatrixType H1,
OptionalMatrixType H2) const override;
/// return the measurement, in the navigation frame
@ -304,7 +304,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
Vector evaluateError(const NavState& nTb, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
@ -384,7 +384,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
Vector evaluateError(const NavState& nTb, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
@ -453,7 +453,7 @@ public:
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const NavState& p, const Point3& bL,
Vector evaluateError(const NavState& nTb, const Point3& bL,
OptionalMatrixType H1,
OptionalMatrixType H2) const override;

2
gtsam/navigation/MagPoseFactor.h
View File

@ -77,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
const Point& direction,
const Point& bias,
const SharedNoiseModel& model,
const std::optional<POSE>& body_P_sensor)
const std::optional<POSE>& body_P_sensor = {})
: Base(model, pose_key),
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
nM_(scale * direction.normalized()),

4
gtsam/navigation/doc/AHRSFactor.ipynb
View File

@ -17,7 +17,7 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
@ -33,7 +33,7 @@
}
],
"source": [
"%pip install --quiet gtsam plotly"
"%pip install --quiet gtsam-develop plotly"
]
},
{

209
gtsam/navigation/doc/AttitudeFactor.ipynb Normal file
View File

@ -0,0 +1,209 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# AttitudeFactor\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/AttitudeFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
"\n",
"## Introduction\n",
"\n",
"The `AttitudeFactor` family in GTSAM provides factors that constrain the orientation (attitude) of a body (`Rot3` or `Pose3`) based on directional measurements. A common use case is constraining roll and pitch using an accelerometer (measuring gravity) or constraining yaw using a magnetometer (measuring the Earth's magnetic field).\n",
"\n",
"This notebook explains the mathematical foundation and usage of these factors."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n",
"\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n",
"\u001b[0mNote: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Foundation\n",
"\n",
"### Concept\n",
"\n",
"The `AttitudeFactor` constrains the rotation $R_{nb}$ (from the body frame $b$ to the navigation frame $n$) such that a known reference direction in the navigation frame (`nRef`) aligns with a measured direction in the body frame (`bMeasured`), when rotated by $R_{nb}$.\n",
"\n",
"The factor enforces that:\n",
"$$ \\text{nRef} \\approx R_{nb} \\cdot \\text{bMeasured} $$ \n",
"\n",
"where:\n",
"- $R_{nb}$ is the rotation matrix representing the orientation from body to navigation frame.\n",
"- `bMeasured` is the direction *vector* measured by the sensor in the *body* frame (e.g., the accelerometer reading, typically normalized to a `Unit3`).\n",
"- `nRef` is the known direction *vector* of the corresponding physical quantity in the *navigation* frame (e.g., the gravity vector, typically normalized to a `Unit3`).\n",
"\n",
"### Error Function\n",
"\n",
"The error function computes the angular difference between the reference direction (`nRef`) and the rotated measured direction ($R_{nb} \\cdot \\text{bMeasured}$). GTSAM uses the `Unit3::error` method, which typically calculates a 2-dimensional tangent space error.\n",
"\n",
"$$ e = \\text{nRef}.\\text{error}(R_{nb} \\cdot \\text{bMeasured}) $$ \n",
"\n",
"This error is minimal (zero) when the rotated body measurement aligns perfectly with the navigation reference direction.\n",
"\n",
"The `attitudeError` function within the base class implements this:\n",
"```cpp\n",
"Vector AttitudeFactor::attitudeError(const Rot3& nRb) const {\n",
" // measured direction in body frame rotated into nav frame\n",
" Unit3 nPredicted = nRb * bMeasured_;\n",
" // error between predicted and reference direction\n",
" return nRef_.error(nPredicted); \n",
"} \n",
"```\n",
"\n",
"### Jacobians\n",
"\n",
"For optimization, the $2 \\times 3$ Jacobian of the error function with respect to the rotation parameters ($R_{nb}$) is required. This is computed using the chain rule, involving the derivative of the rotated vector and the derivative of the `Unit3::error` function.\n",
"\n",
"**Note:** The Jacobian for this specific error function can become zero or ill-defined when the angle between the predicted and reference directions is exactly 180 degrees. The factor behaves best when the initial estimate for $R_{nb}$ is reasonably close (i.e., within the correct hemisphere)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Available Factors\n",
"\n",
"- `Rot3AttitudeFactor`: Constrains a `Rot3` variable.\n",
"- `Pose3AttitudeFactor`: Constrains the rotational part of a `Pose3` variable."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Let's constrain the roll and pitch of a `Pose3` using an accelerometer reading. We assume an ENU navigation frame (Z is up) and the accelerometer measures gravity when stationary."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created Pose3AttitudeFactor:\n",
"Pose3AttitudeFactor on x0\n",
" reference direction in nav frame: : 0\n",
" 0\n",
"-1\n",
" measured direction in body frame: :0.0102036\n",
" 0\n",
"-0.999948\n",
"isotropic dim=2 sigma=0.1\n",
"\n",
"Error at identity pose: [ 0. -0.01020355]\n",
"Error near expected zero pose: [ 0.00999931 -0.01020355]\n"
]
}
],
"source": [
"import numpy as np\n",
"from gtsam import Pose3, Unit3, Rot3, Pose3AttitudeFactor\n",
"from gtsam.symbol_shorthand import X\n",
"from gtsam.noiseModel import Isotropic\n",
"\n",
"# Define the reference direction in the navigation (ENU) frame\n",
"# Gravity points along the negative Z axis in ENU.\n",
"nRef_gravity = Unit3(np.array([0.0, 0.0, -1.0]))\n",
"\n",
"# Define the measured direction in the body frame\n",
"# Assume the accelerometer reading is [0.1, 0.0, -9.8]. \n",
"# GTSAM's Unit3 constructor normalizes this automatically.\n",
"# The factor expects the *direction* the sensor measures in the *body* frame.\n",
"bMeasured_acc = Unit3(np.array([0.1, 0.0, -9.8]))\n",
"\n",
"# Define the noise model for the measurement (2-dimensional error)\n",
"# Example: 0.1 radians standard deviation on the tangent plane error\n",
"attitude_noise_sigma = 0.1\n",
"noise_model = Isotropic.Sigma(2, attitude_noise_sigma)\n",
"\n",
"# Create the factor\n",
"attitude_factor = Pose3AttitudeFactor(X(0), nRef_gravity, \n",
" noise_model, bMeasured_acc)\n",
"\n",
"print(\"Created Pose3AttitudeFactor:\")\n",
"attitude_factor.print()\n",
"\n",
"# Example: Evaluate the error at the identity pose (likely non-zero error)\n",
"identity_pose = Pose3()\n",
"error = attitude_factor.evaluateError(identity_pose)\n",
"print(\"\\nError at identity pose:\", error)\n",
"\n",
"# For zero error, the rotated measurement should align with the reference\n",
"# nRef = R_nb * bMeas => R_nb = nRef * bMeas.inverse() (approx for Unit3)\n",
"# This is complex to solve directly, but optimization finds the R_nb where error is zero.\n",
"# Let's try a pose that should roughly align Z_body with Z_nav (small roll)\n",
"zero_error_pose = Pose3(Rot3.Roll(-0.01), np.zeros(3)) # Approx -0.1/9.8 rad\n",
"error_near_zero = attitude_factor.evaluateError(zero_error_pose)\n",
"print(\"Error near expected zero pose:\", error_near_zero)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Conclusion\n",
"\n",
"The `AttitudeFactor` is a crucial tool for incorporating absolute orientation information from sensors like accelerometers and magnetometers into GTSAM factor graphs. It helps constrain orientation estimates, particularly roll and pitch (using gravity) and potentially yaw (using magnetic north), improving navigation accuracy, especially in GPS-denied scenarios."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [AttitudeFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.h)\n",
"- [AttitudeFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.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
}

169
gtsam/navigation/doc/BarometricFactor.ipynb Normal file
View File

@ -0,0 +1,169 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# BarometricFactor\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/BarometricFactor.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 `BarometricFactor` (contributed by [Peter Milani](https://github.com/pmmilani) in 2021) provides a way to incorporate altitude information derived from barometric pressure measurements into a GTSAM factor graph. It acts as a unary factor on a `Pose3` state variable but also connects to a `double` variable representing a slowly varying atmospheric pressure bias (or equivalently, an altitude offset).\n",
"\n",
"Barometers measure absolute atmospheric pressure. Under the assumption of a standard atmosphere model, this pressure can be converted into an estimate of altitude above sea level. However, local atmospheric pressure changes constantly due to weather, making the direct pressure-to-altitude conversion inaccurate over longer periods. The `BarometricFactor` accounts for this by simultaneously estimating the vehicle's altitude (Z-component of the `Pose3`) and a bias term that absorbs the slow variations in local atmospheric pressure relative to the standard model."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Formulation\n",
"\n",
"### Measurement Model\n",
"\n",
"1. **Pressure to Altitude:** The factor internally converts the input barometric pressure measurement $P_{meas}$ (in kPa) to an altitude estimate $h_{std}$ (in meters) using a standard atmosphere model (approximated by the factor's `heightOut` method, based on [NASA GRC](https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html)). This $h_{std}$ becomes the factor's internal measurement `nT_`.\n",
" $$ h_{std} = \\text{heightOut}(P_{meas}) $$ \n",
"\n",
"2. **Factor Error:** The factor constrains the Z-component of the `Pose3` variable's translation ($p_z$) and the bias variable ($b$). The error is the difference between the altitude predicted by the state and bias, and the altitude derived from the measurement:\n",
" $$ e = (p_z + b) - h_{std} $$ \n",
" where:\n",
" - $p_z$ is the Z-coordinate of the `Pose3` translation.\n",
" - $b$ is the estimated altitude bias (in meters).\n",
" - $h_{std}$ is the altitude calculated from the pressure measurement.\n",
"\n",
"### Bias Modeling\n",
"\n",
"The bias $b$ is typically connected between successive time steps using a `BetweenFactor<double>` with a very small noise model, enforcing that the bias changes slowly over time (approximating a random walk)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Constructor**: `BarometricFactor(poseKey, biasKey, baroIn_kPa, model)`: Creates the factor, taking the `Pose3` key, the `double` bias key, the pressure measurement *in kilopascals (kPa)*, and the 1D noise model for the altitude error.\n",
"- **`evaluateError(pose, bias)`**: Calculates the 1D error $e = (pose.z() + bias) - h_{std}$.\n",
"- **`measurementIn()`**: Returns the internally stored altitude measurement $h_{std}$ (converted from the input pressure).\n",
"- **`heightOut(pressure_kPa)`**: Converts pressure (kPa) to altitude (m) using the standard atmosphere model.\n",
"- **`baroOut(altitude_m)`**: Converts altitude (m) back to pressure (kPa) using the inverse model.\n",
"- **`print` / `equals`**: Standard factor methods."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Assume we have a pressure reading and want to add a factor constraining a `Pose3` and a bias."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created BarometricFactor:\n",
"Barometric Factor on x0Barometric Bias on b0\n",
" Baro measurement: 108.939\n",
" noise model: unit (1) \n",
"\n",
"Internal altitude measurement: 108.94 m\n",
"Current Pose Z: 110.00 m\n",
"Current Bias: -1.50 m\n",
"Predicted Altitude (Pose Z + Bias): 108.50 m\n",
"Error (Predicted - Measured): -0.44 m\n"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam.symbol_shorthand import X, B # Pose key, Bias key\n",
"\n",
"# Define keys\n",
"pose_key = X(0)\n",
"bias_key = B(0)\n",
"\n",
"# Measurement\n",
"pressure_kPa = 100.1 # Example pressure reading in kilopascals\n",
"\n",
"# Noise model on the *altitude* error (e.g., +/- 1 meter sigma)\n",
"altitude_sigma = 1.0 \n",
"noise_model = gtsam.noiseModel.Isotropic.Sigma(1, altitude_sigma)\n",
"\n",
"# Create the factor\n",
"barometric_factor = gtsam.BarometricFactor(pose_key, bias_key, pressure_kPa, noise_model)\n",
"\n",
"print(\"Created BarometricFactor:\")\n",
"barometric_factor.print()\n",
"\n",
"# Internal altitude measurement (converted from pressure)\n",
"internal_altitude_measurement = barometric_factor.measurementIn()\n",
"print(f\"\\nInternal altitude measurement: {internal_altitude_measurement:.2f} m\")\n",
"\n",
"# Example: Evaluate error \n",
"# Assume current state estimate is Pose3 at z=110m and bias= -1.5m\n",
"current_pose = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 110.0]))\n",
"current_bias = -1.5\n",
"\n",
"error = barometric_factor.evaluateError(current_pose, current_bias)\n",
"predicted_altitude = current_pose.z() + current_bias\n",
"print(f\"Current Pose Z: {current_pose.z():.2f} m\")\n",
"print(f\"Current Bias: {current_bias:.2f} m\")\n",
"print(f\"Predicted Altitude (Pose Z + Bias): {predicted_altitude:.2f} m\")\n",
"print(f\"Error (Predicted - Measured): {error[0]:.2f} m\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [BarometricFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.h)\n",
"- [BarometricFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.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
}

238
gtsam/navigation/doc/CombinedImuFactor.ipynb Normal file
View File

@ -0,0 +1,238 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# CombinedImuFactor\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/CombinedImuFactor.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 `CombinedImuFactor` is an advanced IMU factor in GTSAM that offers several improvements over the standard `ImuFactor`:\n",
"1. **Bias Evolution:** It explicitly models the evolution of IMU biases between time steps $i$ and $j$ using a random walk model.\n",
"2. **6-Way Factor:** Consequently, it connects *six* variables: Pose_i, Vel_i, Bias_i, Pose_j, Vel_j, and Bias_j.\n",
"3. **Combined Preintegration:** It uses `PreintegratedCombinedMeasurements` which propagates a full 15x15 covariance matrix, accounting for correlations between the preintegrated state and the biases, as well as the bias random walk noise.\n",
"\n",
"This factor is generally preferred when bias stability is a concern or when modeling the time-varying nature of biases is important for accuracy. It eliminates the need for separate `BetweenFactor`s on bias variables."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Formulation\n",
"\n",
"The `CombinedImuFactor` has a 15-dimensional error vector, conceptually split into two parts:\n",
"\n",
"1. **Preintegration Error (9 dimensions):** This part is identical in concept to the error in `ImuFactor`. It measures the discrepancy between the relative motion predicted by the `PreintegratedCombinedMeasurements` (using the current estimate of $b_i$) and the relative motion implied by the states $X_i = (R_i, p_i, v_i)$ and $X_j = (R_j, p_j, v_j)$.\n",
" $$ e_{imu} = \\text{NavState::Logmap}( \\text{pim.predict}(X_i, b_i)^{-1} \\otimes X_j ) $$ \n",
"\n",
"2. **Bias Random Walk Error (6 dimensions):** This part measures how much the change in bias ($b_j - b_i$) deviates from the expected zero-mean random walk. \n",
" $$ e_{bias} = b_j - b_i $$ \n",
"\n",
"The total error vector is $e = [e_{imu}; e_{bias}]$.\n",
"\n",
"The factor's noise model (derived from `pim.preintMeasCov()`) is a 15x15 matrix that correctly weights these two error components, accounting for the propagated uncertainty from IMU measurements, initial bias uncertainty, and the bias random walk process noise."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Constructor**: `CombinedImuFactor(keyPose_i, keyVel_i, keyPose_j, keyVel_j, keyBias_i, keyBias_j, pim)`: Creates the factor, linking the six state/bias keys and providing the combined preintegrated measurements.\n",
"- **`preintegratedMeasurements()`**: Returns a const reference to the stored `PreintegratedCombinedMeasurements` object.\n",
"- **`evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i, bias_j)`**: Calculates the 15-dimensional error vector given the current values of the connected variables. Also computes Jacobians if requested.\n",
"- **`noiseModel()`**: Returns the 15x15 noise model associated with the preintegrated measurement and bias evolution uncertainty.\n",
"- **`print` / `equals`**: Standard factor methods."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Create combined parameters, create a combined PIM object, define keys (including two bias keys), and construct the factor."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created CombinedImuFactor:\n",
"CombinedImuFactor(x0,v0,x1,v1,b0,b1)\n",
" preintegrated measurements:\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 [ 0.00011 0 0 0 1.53772e-06 0 0 4.85595e-05 0 0 0 0 4.5e-11 0 0\n",
" 0 0.00011 0 -1.53772e-06 0 0 -4.85595e-05 0 0 0 0 0 0 4.5e-11 0\n",
" 0 0 0.00011 0 0 0 0 0 0 0 0 0 0 0 4.5e-11\n",
" 0 -1.53772e-06 0 3.69908e-06 0 0 5.60718e-05 0 0 1.425e-10 0 0 0 -2.6487e-13 0\n",
" 1.53772e-06 0 0 0 3.69908e-06 0 0 5.60718e-05 0 0 1.425e-10 0 2.6487e-13 0 0\n",
" 0 0 0 0 0 3.6585e-06 0 0 5.5e-05 0 0 1.425e-10 0 0 0\n",
" 0 -4.85595e-05 0 5.60718e-05 0 0 0.00113017 0 0 4.5e-09 0 0 0 -1.1772e-11 0\n",
" 4.85595e-05 0 0 0 5.60718e-05 0 0 0.00113017 0 0 4.5e-09 0 1.1772e-11 0 0\n",
" 0 0 0 0 0 5.5e-05 0 0 0.0011 0 0 4.5e-09 0 0 0\n",
" 0 0 0 1.425e-10 0 0 4.5e-09 0 0 1e-07 0 0 0 0 0\n",
" 0 0 0 0 1.425e-10 0 0 4.5e-09 0 0 1e-07 0 0 0 0\n",
" 0 0 0 0 0 1.425e-10 0 0 4.5e-09 0 0 1e-07 0 0 0\n",
" 4.5e-11 0 0 0 2.6487e-13 0 0 1.1772e-11 0 0 0 0 1e-09 0 0\n",
" 0 4.5e-11 0 -2.6487e-13 0 0 -1.1772e-11 0 0 0 0 0 0 1e-09 0\n",
" 0 0 4.5e-11 0 0 0 0 0 0 0 0 0 0 0 1e-09 ]\n",
" noise model: Gaussian [\n",
"\t96.6351, 0, 0, 0, 91.8245, 0, -0, -8.70782, -0, 0, 0.261002, 0, -4.27039, 0, 0;\n",
"\t0, 96.6351, 0, -91.8245, 0, 0, 8.70782, -0, -0, -0.261002, 0, 0, 0, -4.27039, 0;\n",
"\t0, 0, 95.3463, 0, 0, 0, -0, -0, -0, 0, 0, 0, 0, 0, -4.29058;\n",
"\t0, 0, 0, 1044.19, 0, 0, -51.806, -0, -0, 0.843301, 0, 0, 0, -0.333286, 0;\n",
"\t0, 0, 0, 0, 1044.19, 0, -0, -51.806, -0, 0, 0.843301, 0, 0.333286, 0, 0;\n",
"\t0, 0, 0, 0, 0, 1049.15, -0, -0, -52.4575, 0, 0, 0.865549, 0, 0, 0;\n",
"\t0, 0, 0, 0, 0, 0, 29.746, -0, -0, -1.33857, 0, 0, 0, 0.35017, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 29.746, -0, 0, -1.33857, 0, -0.35017, 0, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 0, 30.1511, 0, 0, -1.3568, 0, 0, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, 0, 0, 5.26625e-20, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, -5.26625e-20, 0, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, 0, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8, 0, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8, 0;\n",
"\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8\n",
"]\n",
"\n",
"Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]\n",
"Factor error (0.5 * ||error||^2_Sigma): 0.0\n"
]
}
],
"source": [
"import numpy as np\n",
"from gtsam import (PreintegrationCombinedParams,\n",
" PreintegratedCombinedMeasurements, CombinedImuFactor,\n",
" NonlinearFactorGraph, Values, Pose3, NavState)\n",
"from gtsam.imuBias import ConstantBias\n",
"from gtsam.symbol_shorthand import X, V, B\n",
"\n",
"# 1. Create Combined Parameters and PIM (as in PreintegratedCombinedMeasurements example)\n",
"params = PreintegrationCombinedParams.MakeSharedU(9.81)\n",
"accel_noise_sigma = 0.1\n",
"gyro_noise_sigma = 0.01\n",
"params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
"params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
"params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
"bias_acc_rw_sigma = 0.001\n",
"bias_gyro_rw_sigma = 0.0001\n",
"params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)\n",
"params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)\n",
"initial_bias_cov = np.diag(np.full(6, 1e-3)) # Example initial bias uncertainty\n",
"params.setBiasAccOmegaInit(initial_bias_cov)\n",
"bias_hat = ConstantBias()\n",
"pim = PreintegratedCombinedMeasurements(params, bias_hat)\n",
"\n",
"# Integrate some dummy measurements\n",
"dt = 0.01\n",
"acc_meas = np.array([0.0, 0.0, -9.81]) # Stationary\n",
"gyro_meas = np.array([0.0, 0.0, 0.0]) # Stationary\n",
"for _ in range(10):\n",
" pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
"\n",
"# 2. Define Symbolic Keys using shorthand\n",
"# Keys: X(0), V(0), B(0) for time i\n",
"# X(1), V(1), B(1) for time j\n",
"\n",
"# 3. Create the CombinedImuFactor\n",
"# The 15x15 noise model is automatically derived from pim.preintMeasCov()\n",
"combined_imu_factor = CombinedImuFactor(\n",
" X(0), V(0),\n",
" X(1), V(1),\n",
" B(0), B(1),\n",
" pim)\n",
"\n",
"print(\"Created CombinedImuFactor:\")\n",
"combined_imu_factor.print()\n",
"\n",
"# 4. Example: Evaluate error with perfect states & no bias change (should be near zero)\n",
"graph = NonlinearFactorGraph()\n",
"graph.add(combined_imu_factor)\n",
"\n",
"values = Values()\n",
"pose_i = Pose3()\n",
"vel_i = np.zeros(3)\n",
"bias_i = ConstantBias() # Matches bias_hat used in PIM\n",
"bias_j = bias_i # Assume no bias change for zero error check\n",
"\n",
"# Predict state j using the PIM\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",
"pose_j = nav_state_j.pose()\n",
"vel_j = nav_state_j.velocity()\n",
"\n",
"values.insert(X(0), pose_i)\n",
"values.insert(V(0), vel_i)\n",
"values.insert(B(0), bias_i)\n",
"values.insert(X(1), pose_j)\n",
"values.insert(V(1), vel_j)\n",
"values.insert(B(1), bias_j)\n",
"\n",
"error_vector = combined_imu_factor.evaluateError(\n",
" pose_i, vel_i, pose_j, vel_j, bias_i, bias_j)\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": {},
"source": [
"## Source\n",
"- [CombinedImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.h)\n",
"- [CombinedImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.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
}

176
gtsam/navigation/doc/ConstantVelocityFactor.ipynb Normal file
View File

@ -0,0 +1,176 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# ConstantVelocityFactor\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/ConstantVelocityFactor.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 `ConstantVelocityFactor` (contributed by [Asa Hammond](https://www.linkedin.com/in/asahammond/) in 2021) is a simple motion model factor that connects two `NavState` variables at different times, $t_i$ and $t_j$. It enforces the assumption that the velocity remained constant between the two time steps.\n",
"\n",
"Given `NavState` $X_i$ (containing pose $T_i$ and velocity $v_i$) and `NavState` $X_j$ (containing pose $T_j$ and velocity $v_j$), and the time difference $\\Delta t = t_j - t_i$, this factor penalizes deviations from the constant velocity prediction."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Formulation\n",
"\n",
"The factor uses the `NavState::update` method (or equivalent logic internally) to predict the state at time $t_j$ based on the state at $t_i$ and the assumption of constant velocity (and zero acceleration/angular velocity). Let this prediction be $X_{j, pred}$:\n",
"$$ X_{j, pred} = X_i . \\text{update}(\\text{accel}=0, \\text{omega}=0, \\Delta t) $$ \n",
"Essentially, this integrates the velocity $v_i$ over $\\Delta t$ to predict the change in position, while keeping orientation and velocity constant:\n",
"$$ R_{j, pred} = R_i $$ \n",
"$$ v_{j, pred} = v_i $$ \n",
"$$ p_{j, pred} = p_i + R_i (v_i^{body}) \\Delta t \\quad \\text{(using body velocity update)} $$ \n",
"\n",
"The factor's 9-dimensional error $e$ is the difference between the predicted state $X_{j, pred}$ and the actual state $X_j$, expressed in the tangent space at $X_{j,pred}$:\n",
"$$ e = \\text{localCoordinates}_{X_{j, pred}}(X_j) $$ \n",
"The noise model associated with the factor determines how strongly deviations from this constant velocity prediction are penalized."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Constructor**: `ConstantVelocityFactor(key1, key2, dt, model)`: Creates the factor connecting the `NavState` at `key1` (time $i$) and `key2` (time $j$), given the time difference `dt` and a 9D noise model.\n",
"- **`evaluateError(state1, state2)`**: Calculates the 9D error vector based on the constant velocity prediction from `state1` to `state2` over the stored `dt`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"This factor is often used as a simple process model between consecutive states when higher-fidelity IMU integration is not available or needed."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created ConstantVelocityFactor:\n",
" keys = { x0 x1 }\n",
" noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.1; 0.1; 0.1; 0.05; 0.05; 0.05];\n",
"\n",
"Error for perfect prediction (should be zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]\n",
"Error for velocity change: [0. 0. 0. 0. 0. 0. 0. 0.1 0. ]\n",
"Error for position change: [0. 0. 0. 0. 0.05 0. 0. 0. 0. ]\n"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam.symbol_shorthand import X # Using X for NavState keys here\n",
"\n",
"# Define keys for two NavStates\n",
"key1 = X(0)\n",
"key2 = X(1)\n",
"\n",
"# Time difference between states\n",
"dt = 0.1 # seconds\n",
"\n",
"# Define a noise model - how much deviation from constant velocity is allowed?\n",
"# Example: Tighter on rotation/velocity, looser on position change due to velocity\n",
"rot_sigma = 0.01 # rad\n",
"pos_sigma = 0.1 # m \n",
"vel_sigma = 0.05 # m/s\n",
"sigmas = np.concatenate([\n",
" np.full(3, rot_sigma), \n",
" np.full(3, pos_sigma),\n",
" np.full(3, vel_sigma)\n",
"])\n",
"noise_model = gtsam.noiseModel.Diagonal.Sigmas(sigmas)\n",
"\n",
"# Create the factor\n",
"cv_factor = gtsam.ConstantVelocityFactor(key1, key2, dt, noise_model)\n",
"\n",
"print(\"Created ConstantVelocityFactor:\")\n",
"cv_factor.print()\n",
"\n",
"# --- Example Evaluation ---\n",
"# State 1: At origin, moving along +X at 1 m/s\n",
"pose1 = gtsam.Pose3()\n",
"vel1 = np.array([1.0, 0.0, 0.0])\n",
"state1 = gtsam.NavState(pose1, vel1)\n",
"\n",
"# State 2: Exactly matches constant velocity prediction\n",
"pose2 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.0, 0.0]))\n",
"vel2 = vel1 # Constant velocity\n",
"state2_perfect = gtsam.NavState(pose2, vel2)\n",
"\n",
"error_perfect = cv_factor.evaluateError(state1, state2_perfect)\n",
"print(\"\\nError for perfect prediction (should be zero):\", error_perfect)\n",
"\n",
"# State 3: Velocity changed slightly\n",
"vel3 = np.array([1.0, 0.1, 0.0]) # Added Y velocity\n",
"state3_vel_change = gtsam.NavState(pose2, vel3) # Keep predicted pose\n",
"\n",
"error_vel_change = cv_factor.evaluateError(state1, state3_vel_change)\n",
"print(\"Error for velocity change:\", error_vel_change)\n",
"\n",
"# State 4: Position is slightly off prediction\n",
"pose4 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.05, 0.0])) # Y pos is off\n",
"state4_pos_change = gtsam.NavState(pose4, vel2) # Keep velocity\n",
"\n",
"error_pos_change = cv_factor.evaluateError(state1, state4_pos_change)\n",
"print(\"Error for position change:\", error_pos_change)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [ConstantVelocityFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ConstantVelocityFactor.h)"
]
}
],
"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
}

244
gtsam/navigation/doc/GPSFactor.ipynb Normal file
View File

@ -0,0 +1,244 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# GPSFactor Family\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/GPSFactor.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 `GPSFactor` family provides factors for incorporating Global Positioning System (GPS) measurements into a GTSAM factor graph. GPS typically provides measurements of position in Latitude/Longitude/Height. These GPS factors, however, assume the GPS measurement has been converted into a local Cartesian **navigation frame** (e.g., [ENU, NED, or ECEF](https://dirsig.cis.rit.edu/docs/new/coordinates.html)).\n",
"\n",
"Different variants exist to handle:\n",
"- State type: `Pose3` or `NavState`.\n",
"- Lever arm: Whether the GPS antenna is offset from the body frame origin.\n",
"- Calibration: Whether the lever arm itself is being estimated."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Factor Variants\n",
"\n",
"**For `Pose3` states:**\n",
"\n",
"- **`GPSFactor`**: \n",
" - Connects to: `Pose3` key.\n",
" - Assumes: Zero lever arm (GPS measurement corresponds directly to the `Pose3` origin).\n",
" - Measurement: 3D position (`Point3`) in the navigation frame.\n",
" - Error: $p_{pose} - p_{measured}$\n",
"\n",
"- **`GPSFactorArm`**: \n",
" - Connects to: `Pose3` key.\n",
" - Assumes: Known, fixed lever arm (`bL` vector in the body frame).\n",
" - Measurement: 3D position (`Point3`) in the navigation frame.\n",
" - Error: $(p_{pose} + R_{nb} \\cdot bL) - p_{measured}$\n",
"\n",
"- **`GPSFactorArmCalib`**: \n",
" - Connects to: `Pose3` key, `Point3` key (for the lever arm).\n",
" - Assumes: Lever arm (`bL`) is unknown and estimated.\n",
" - Measurement: 3D position (`Point3`) in the navigation frame.\n",
" - Error: $(p_{pose} + R_{nb} \\cdot bL_{estimated}) - p_{measured}$\n",
"\n",
"**For `NavState` states:**\n",
"\n",
"- **`GPSFactor2`**: Like `GPSFactor` but connects to a `NavState` key.\n",
"- **`GPSFactor2Arm`**: Like `GPSFactorArm` but connects to a `NavState` key.\n",
"- **`GPSFactor2ArmCalib`**: Like `GPSFactorArmCalib` but connects to a `NavState` key and a `Point3` lever arm key.\n",
"\n",
"(The '2' suffix historically denoted factors using `NavState`)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Formulation (GPSFactorArm Example)\n",
"\n",
"Let:\n",
"- $T_{nb} = (R_{nb}, p_{nb})$ be the `Pose3` state (body frame $b$ in navigation frame $n$).\n",
"- $L_b$ be the known lever arm vector from the body origin to the GPS antenna, expressed in the body frame.\n",
"- $p_{gps}$ be the measured GPS position in the navigation frame.\n",
"\n",
"The predicted position of the GPS antenna in the navigation frame is:\n",
"$$ p_{ant, pred} = p_{nb} + R_{nb} \\cdot L_b $$ \n",
"\n",
"The factor's 3D error vector is the difference between the predicted antenna position and the measured GPS position:\n",
"$$ e = p_{ant, pred} - p_{gps} $$ \n",
"\n",
"The noise model reflects the uncertainty of the $p_{gps}$ measurement in the navigation frame."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API (Common Patterns)\n",
"\n",
"- **Constructor**: Takes the relevant key(s), the measured `Point3` position `gpsIn` (in nav frame), the noise model, and potentially the `leverArm` (`Point3` in body frame).\n",
"- **`evaluateError(...)`**: Calculates the 3D error vector based on the connected state variable(s) and the measurement.\n",
"- **`measurementIn()`**: Returns the stored `Point3` measurement.\n",
"- **`leverArm()`** (For Arm variants): Returns the stored `Point3` lever arm."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example (GPSFactor and GPSFactorArm)\n",
"\n",
"Assume we have a GPS reading converted to a local ENU frame."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created GPSFactor (zero lever arm):\n",
"GPSFactor on x0\n",
" GPS measurement: 10.5\n",
"20.2\n",
" 5.1\n",
" noise model: diagonal sigmas [0.5; 0.5; 1];\n",
"\n",
"GPSFactor Error: [-0.5 -0.2 -0.1]\n",
"\n",
"Created GPSFactorArm:\n",
"GPSFactorArm on x0\n",
" GPS measurement: 10.5 20.2 5.1\n",
" Lever arm: -0.1 0 0.05\n",
" noise model: diagonal sigmas [0.5; 0.5; 1];\n",
"\n",
"GPSFactorArm Error: [-0.6 -0.2 -0.05]\n",
" ( Pose: [10. 20. 5.] )\n",
" ( Lever Arm: [-0.1 0. 0.05] )\n",
" ( Predicted Antenna Pos: [ 9.9 20. 5.05] )\n",
" ( Measured GPS Pos: [10.5 20.2 5.1] )\n",
"\n",
"Created GPSFactorArmCalib:\n",
"GPSFactorArmCalib on x0\n",
" GPS measurement: 10.5 20.2 5.1\n",
" noise model: diagonal sigmas [0.5; 0.5; 1];\n"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam.symbol_shorthand import X, L # Pose key, Lever arm key\n",
"\n",
"# --- Setup ---\n",
"pose_key = X(0)\n",
"\n",
"# GPS Measurement in local ENU frame (meters)\n",
"gps_measurement_enu = gtsam.Point3(10.5, 20.2, 5.1)\n",
"\n",
"# Noise model for GPS measurement (e.g., 0.5m horizontal, 1.0m vertical sigma)\n",
"gps_sigmas = np.array([0.5, 0.5, 1.0])\n",
"gps_noise_model = gtsam.noiseModel.Diagonal.Sigmas(gps_sigmas)\n",
"\n",
"# --- Scenario 1: GPSFactor (Zero Lever Arm) ---\n",
"gps_factor_zero_arm = gtsam.GPSFactor(pose_key, gps_measurement_enu, gps_noise_model)\n",
"print(\"Created GPSFactor (zero lever arm):\")\n",
"gps_factor_zero_arm.print()\n",
"\n",
"# Evaluate error: Error is difference between pose translation and measurement\n",
"test_pose1 = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(10.0, 20.0, 5.0))\n",
"error1 = gps_factor_zero_arm.evaluateError(test_pose1)\n",
"print(\"\\nGPSFactor Error:\", error1) # Expected: [0.5, 0.2, 0.1]\n",
"\n",
"# --- Scenario 2: GPSFactorArm (Known Lever Arm) ---\n",
"# Assume antenna is 10cm behind and 5cm above the body origin\n",
"lever_arm_body = gtsam.Point3(-0.1, 0.0, 0.05) \n",
"\n",
"gps_factor_with_arm = gtsam.GPSFactorArm(pose_key, gps_measurement_enu, \n",
" lever_arm_body, gps_noise_model)\n",
"print(\"\\nCreated GPSFactorArm:\")\n",
"gps_factor_with_arm.print()\n",
"\n",
"# Evaluate error: Error is difference between (pose + R*lever_arm) and measurement\n",
"# Use the same test pose as before\n",
"predicted_antenna_pos = test_pose1.transformFrom(lever_arm_body)\n",
"error2 = gps_factor_with_arm.evaluateError(test_pose1)\n",
"print(\"\\nGPSFactorArm Error:\", error2) \n",
"print(\" ( Pose: \", test_pose1.translation() , \")\")\n",
"print(\" ( Lever Arm: \", lever_arm_body, \")\")\n",
"print(\" ( Predicted Antenna Pos: \", predicted_antenna_pos, \")\")\n",
"print(\" ( Measured GPS Pos: \", gps_measurement_enu, \")\")\n",
"# Expected: predicted_antenna_pos - gps_measurement_enu \n",
"# = [9.9, 20.0, 5.05] - [10.5, 20.2, 5.1] = [-0.6, -0.2, -0.05]\n",
"\n",
"# --- Scenario 3: GPSFactorArmCalib (Example Setup - Not Evaluated) ---\n",
"lever_arm_key = L(0) # Key for the unknown lever arm\n",
"gps_factor_calib = gtsam.GPSFactorArmCalib(pose_key, lever_arm_key, \n",
" gps_measurement_enu, gps_noise_model)\n",
"print(\"\\nCreated GPSFactorArmCalib:\")\n",
"gps_factor_calib.print()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Coordinate Frames\n",
"\n",
"It's crucial to ensure consistency in coordinate frames:\n",
"- **GPS Measurement (`gpsIn`)**: Must be provided in the chosen local Cartesian **navigation frame** (e.g., ENU, NED).\n",
"- **Lever Arm (`leverArm`)**: Must be provided in the **body frame**.\n",
"- **Pose/NavState Variables**: Represent the pose of the body frame in the navigation frame ($T_{nb}$)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [GPSFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.h)\n",
"- [GPSFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.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
}

309
gtsam/navigation/doc/ImuFactor.ipynb Normal file
View File

@ -0,0 +1,309 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# ImuFactor\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/ImuFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
"\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",
"2. Velocity at time $i$ (`Vector3`)\n",
"3. Pose at time $j$ (`Pose3`)\n",
"4. Velocity at time $j$ (`Vector3`)\n",
"5. IMU Bias at time $i$ (`imuBias::ConstantBias`)\n",
"\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",
"### 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."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n",
"\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n",
"\u001b[0mNote: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Formulation\n",
"\n",
"Let $X_i = (R_i, p_i, v_i)$ be the state (Attitude, Position, Velocity) at time $i$, and $b_i$ be the bias estimate at time $i$.\n",
"Let $X_j = (R_j, p_j, v_j)$ be the state at time $j$.\n",
"\n",
"The `PreintegratedImuMeasurements` object (`pim`) provides:\n",
"- $\\Delta \\tilde{R}_{ij}(b_{est})$, $\\Delta \\tilde{p}_{ij}(b_{est})$, $\\Delta \\tilde{v}_{ij}(b_{est})$: Preintegrated measurements calculated using the fixed bias estimate $b_{est}$ (`pim.biasHat()`).\n",
"- Jacobians of the $\\Delta$ terms with respect to the bias.\n",
"\n",
"The factor first uses the `pim` object's `predict` method (or equivalent calculations) to find the predicted state $X_{j,pred}$ based on $X_i$ and the *current* estimate of the bias $b_i$ from the factor graph values:\n",
"$$ X_{j,pred} = \\text{pim.predict}(X_i, b_i) $$ \n",
"This prediction internally applies first-order corrections to the stored $\\Delta$ values based on the difference between $b_i$ and $b_{est}$.\n",
"\n",
"The factor's 9-dimensional error vector $e$ is then calculated as the difference between the predicted state $X_{j,pred}$ and the actual state $X_j$ in the tangent space of $X_{j,pred}$:\n",
"$$ e = \\text{Logmap}_{X_{j,pred}}(X_j) = X_{j,pred}^{-1} \\otimes X_j \\quad \\text{(Conceptual Lie notation)} $$ \n",
"Or, more explicitly using the `NavState::Logmap` definition:\n",
"$$ e = \\text{NavState::Logmap}(X_{j,pred}.inverse() * X_j) $$ \n",
"This error vector has components corresponding to errors in rotation (3), position (3), and velocity (3)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Constructor**: `ImuFactor(keyPose_i, keyVel_i, keyPose_j, keyVel_j, keyBias_i, pim)`: Creates the factor, linking the five state/bias keys and providing the preintegrated measurements.\n",
"- **`preintegratedMeasurements()`**: Returns a const reference to the stored `PreintegratedImuMeasurements` object.\n",
"- **`evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i)`**: Calculates the 9-dimensional error vector given the current values of the connected variables. Also computes Jacobians if requested.\n",
"- **`noiseModel()`**: Returns the noise model associated with the preintegrated measurement uncertainty (derived from `pim.preintMeasCov()`).\n",
"- **`print` / `equals`**: Standard factor methods."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Create parameters, a PIM object, define keys, and construct the factor."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created ImuFactor:\n",
"ImuFactor(x0,v0,x1,v1,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 PreintegrationParams, PreintegratedImuMeasurements, ImuFactor\n",
"from gtsam import NonlinearFactorGraph, Values, NavState, Pose3\n",
"from gtsam.symbol_shorthand import X,V,B\n",
"from gtsam.imuBias import ConstantBias\n",
"import numpy as np\n",
"\n",
"# 1. Create Parameters and PIM (as in PreintegratedImuMeasurements example)\n",
"params = PreintegrationParams.MakeSharedU(9.81)\n",
"accel_noise_sigma = 0.1\n",
"gyro_noise_sigma = 0.01\n",
"params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
"params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
"params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
"bias_hat = ConstantBias() # Assume zero bias used for preintegration\n",
"pim = PreintegratedImuMeasurements(params, bias_hat)\n",
"\n",
"# Integrate some dummy measurements\n",
"dt = 0.01\n",
"acc_meas = np.array([0.0, 0.0, -9.81]) # Stationary\n",
"gyro_meas = np.array([0.0, 0.0, 0.0]) # Stationary\n",
"for _ in range(10):\n",
" pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
"\n",
"# 2. Symbolic Keys will be X(0), V(0), X(1), V(1), B(0)\n",
"\n",
"# 3. Create the ImuFactor\n",
"# The noise model is automatically derived from pim.preintMeasCov()\n",
"imu_factor = ImuFactor(X(0), V(0), X(1), V(1), B(0), pim)\n",
"\n",
"print(\"Created ImuFactor:\")\n",
"imu_factor.print()\n",
"\n",
"# 4. Example: Evaluate error with perfect states (should be near zero)\n",
"graph = NonlinearFactorGraph()\n",
"graph.add(imu_factor)\n",
"\n",
"values = Values()\n",
"pose_i = Pose3() # Identity\n",
"vel_i = np.zeros(3)\n",
"bias_i = ConstantBias() # Zero bias\n",
"\n",
"# Predict state j using the PIM and the *same* bias used for integration\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",
"pose_j = nav_state_j.pose()\n",
"vel_j = nav_state_j.velocity()\n",
"\n",
"values.insert(X(0), pose_i)\n",
"values.insert(V(0), vel_i)\n",
"values.insert(X(1), pose_j)\n",
"values.insert(V(1), vel_j)\n",
"values.insert(B(0), bias_i)\n",
"\n",
"error_vector = imu_factor.evaluateError(pose_i, vel_i, pose_j, vel_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": {},
"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": {},
"source": [
"## Source\n",
"- [ImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.h)\n",
"- [ImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.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
}

226
gtsam/navigation/doc/MagFactor.ipynb Normal file
View File

@ -0,0 +1,226 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# MagFactor Family\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/MagFactor.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 `MagFactor` family provides factors for incorporating magnetometer measurements, which sense the Earth's magnetic field, into a GTSAM factor graph. These factors are primarily used to constrain the orientation (mostly \"yaw\") of a body or sensor.\n",
"\n",
"Magnetometers measure the local magnetic field vector in the sensor's own frame. The factors relate this measurement to the known (or estimated) magnetic field direction in the navigation frame via the body's rotation.\n",
"\n",
"Several variants exist depending on what is known and what is being estimated:"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Measurement Model\n",
"\n",
"The general model assumed by these factors is:\n",
"$$ bM_{measured} = R_{bn} \\cdot (s \\cdot \\hat{d}_n) + b $$ \n",
"where:\n",
"- $bM_{measured}$: The 3D magnetic field vector measured by the sensor, in the **body frame**.\n",
"- $R_{bn}$: The rotation matrix from the navigation frame ($n$) to the body frame ($b$). This is the *inverse* of the rotation usually stored in `Pose3` or `NavState` ($R_{nb}$).\n",
"- $s$: A scale factor relating the magnetic field strength to the magnetometer's output units (e.g., nT).\n",
"- $\\hat{d}_n$: The unit vector representing the direction of the Earth's magnetic field in the **navigation frame**.\n",
"- $b$: A 3D additive bias vector in the **body frame**.\n",
"\n",
"The factor error is typically calculated as:\n",
"$$ e = bM_{measured} - [ R_{bn} \\cdot (s \\cdot \\hat{d}_n) + b ] $$ \n",
"Different factors treat different components ($R_{bn}$, $s$, $\\hat{d}_n$, $b$) as known constants or as variables to be estimated."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Factor Variants\n",
"\n",
"- **`MagFactor`**: \n",
" - Estimates: `Rot2` (intended for yaw $R_{nb}$).\n",
" - Assumes Known: `scale`, `direction`, `bias`.\n",
" - Note: Uses `Rot2` which might be limiting unless the sensor is always level.\n",
"\n",
"- **`MagFactor1`**: \n",
" - Estimates: `Rot3` ($R_{nb}$).\n",
" - Assumes Known: `scale`, `direction`, `bias`.\n",
" - This is often the most practical factor for direct orientation estimation when calibration is known.\n",
"\n",
"- **`MagFactor2`**: \n",
" - Estimates: `nM` (`Point3`, the scaled field vector $s \\cdot \\hat{d}_n$ in nav frame), `bias` (`Point3`).\n",
" - Assumes Known: `Rot3` ($R_{nb}$).\n",
" - Useful for calibrating the local field and bias if orientation is known (e.g., from other sensors).\n",
"\n",
"- **`MagFactor3`**: \n",
" - Estimates: `scale` (`double`), `direction` (`Unit3`), `bias` (`Point3`).\n",
" - Assumes Known: `Rot3` ($R_{nb}$).\n",
" - Provides full calibration of scale, direction, and bias when orientation is known.\n",
"\n",
"- **`MagPoseFactor<POSE>`**: (Separate Header: `MagPoseFactor.h`)\n",
" - Estimates: `Pose2` or `Pose3`.\n",
" - Assumes Known: `scale`, `direction`, `bias`, optional `body_P_sensor`.\n",
" - Similar to `MagFactor1` but works directly on `Pose` types and handles sensor-to-body transforms."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API (MagFactor1 Example)\n",
"\n",
"- **Constructor**: `MagFactor1(key, measured, scale, direction, bias, model)`: Creates the factor connecting the `Rot3` key, given the measurement, known calibration parameters, and noise model.\n",
"- **`evaluateError(nRb)`**: Calculates the 3D error vector $e$ based on the current `Rot3` estimate $R_{nb}$."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example (MagFactor1)\n",
"\n",
"Using a magnetometer to help estimate a `Rot3` orientation, assuming known calibration."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created MagFactor1:\n",
" keys = { r0 }\n",
"isotropic dim=3 sigma=50\n",
"\n",
"Error at ground truth rotation (should be zero): [0. 0. 0.]\n",
"Error at test rotation: [1034.79105955 1628.05638524 0. ]\n"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam.symbol_shorthand import R # Rotation key\n",
"\n",
"# --- Assumed Known Calibration & Field ---\n",
"# Local magnetic field direction in navigation frame (e.g., NED)\n",
"# Example: From NOAA for specific location/date\n",
"# Field Strength: 48343.4 nT\n",
"# Declination: -4.94 deg -> Angle from North towards West\n",
"# Inclination: 62.78 deg -> Angle below horizontal\n",
"declination_rad = np.deg2rad(-4.94)\n",
"inclination_rad = np.deg2rad(62.78)\n",
"field_strength_nT = 48343.4\n",
"\n",
"# Convert Dec/Inc to NED vector components\n",
"north_comp = np.cos(inclination_rad) * np.cos(declination_rad)\n",
"east_comp = np.cos(inclination_rad) * np.sin(declination_rad)\n",
"down_comp = np.sin(inclination_rad)\n",
"n_direction = gtsam.Unit3(np.array([north_comp, east_comp, down_comp]))\n",
"\n",
"# Assume scale factor converts unit vector to nT (can be absorbed if field strength is used)\n",
"mag_scale = field_strength_nT\n",
"\n",
"# Assume known magnetometer bias in body frame (nT)\n",
"mag_bias_body = gtsam.Point3(10.0, -5.0, 2.0) \n",
"\n",
"# --- Simulation: Generate Measurement ---\n",
"# Assume a ground truth rotation (e.g., 30 deg yaw right in NED)\n",
"truth_nRb = gtsam.Rot3.Yaw(np.deg2rad(30)) \n",
"truth_bRn = truth_nRb.inverse()\n",
"\n",
"# Calculate the expected magnetic field in the body frame (ideal, before bias)\n",
"n_field_vector = mag_scale * n_direction.point3()\n",
"b_field_ideal = truth_bRn.rotate(n_field_vector)\n",
"\n",
"# Calculate the measured value including bias\n",
"b_measured = b_field_ideal + mag_bias_body\n",
"\n",
"# --- Factor Creation ---\n",
"rot_key = R(0)\n",
"\n",
"# Noise model for the magnetometer measurement (nT)\n",
"mag_noise_sigma = 50.0 # nT\n",
"noise_model = gtsam.noiseModel.Isotropic.Sigma(3, mag_noise_sigma)\n",
"\n",
"# Create MagFactor1\n",
"mag_factor = gtsam.MagFactor1(rot_key, b_measured, mag_scale, \n",
" n_direction, mag_bias_body, noise_model)\n",
"\n",
"print(\"Created MagFactor1:\")\n",
"mag_factor.print()\n",
"\n",
"# --- Evaluate Error ---\n",
"# Evaluate at the ground truth rotation (error should be zero)\n",
"error_at_truth = mag_factor.evaluateError(truth_nRb)\n",
"print(\"\\nError at ground truth rotation (should be zero):\", error_at_truth)\n",
"\n",
"# Evaluate at a different rotation (error should be non-zero)\n",
"test_nRb = gtsam.Rot3.Yaw(np.deg2rad(25)) # Slightly off\n",
"error_at_test = mag_factor.evaluateError(test_nRb)\n",
"print(\"Error at test rotation:\", error_at_test)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Important Notes\n",
"- **Coordinate Frames**: Be very careful with navigation frame (NED vs ENU) and body frame conventions. Ensure the `direction` vector and the `Rot3`/`Pose3` variables use the same navigation frame. The `measured` and `bias` are typically in the body frame.\n",
"- **Units**: Ensure consistency between the `scale`, `bias`, `measured` values and the noise model sigma (e.g., all in nanoTesla (nT)).\n",
"- **Calibration**: Accurate knowledge of `scale`, `direction`, and `bias` is crucial for `MagFactor1` and `MagPoseFactor`. If these are unknown, consider using `MagFactor2`/`MagFactor3` or online calibration techniques."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [MagFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/MagFactor.h)"
]
}
],
"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
}

205
gtsam/navigation/doc/MagPoseFactor.ipynb Normal file
View File

@ -0,0 +1,205 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# MagPoseFactor\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/MagPoseFactor.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 `MagPoseFactor<POSE>` is a templated factor designed to constrain the orientation component of a `Pose2` or `Pose3` variable using magnetometer readings. It's similar in purpose to `MagFactor1` but operates directly on pose types and explicitly handles an optional transformation between the body frame and the sensor frame.\n",
"\n",
"It assumes the magnetometer calibration parameters (scale, bias) and the local magnetic field direction are known."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Measurement Model\n",
"\n",
"The underlying model is the same as for `MagFactor`:\n",
"$$ bM_{measured} = R_{bn} \\cdot (s \\cdot \\hat{d}_n) + b $$ \n",
"However, `MagPoseFactor` allows specifying an optional `body_P_sensor` transform. If provided, the factor internally adjusts the measurement and bias to be consistent with the body frame before calculating the error.\n",
"\n",
"Let:\n",
"- $T_{nb} = (R_{nb}, p_{nb})$ be the `Pose` state.\n",
"- $T_{bs}$ be the pose of the sensor in the body frame (`body_P_sensor`). If not given, $T_{bs}$ is identity.\n",
"- $sM_{measured}$ be the raw measurement in the sensor frame.\n",
"- $sB$ be the bias in the sensor frame.\n",
"- $s$, $\\hat{d}_n$ be the known scale and nav-frame direction.\n",
"\n",
"The factor computes the predicted measurement *in the body frame*:\n",
"$$ bM_{predicted} = R_{bs} [ R_{sn} (s \\cdot \\hat{d}_n) + sB ] $$ \n",
"where $R_{sn} = R_{sb} R_{bn} = R_{bs}^T \\cdot R_{nb}^T$.\n",
"\n",
"Alternatively, and perhaps more clearly, it predicts the field in the sensor frame and compares it to the measurement:\n",
"$$ sM_{predicted} = R_{sn} (s \\cdot \\hat{d}_n) + sB $$ \n",
"$$ e = sM_{measured} - sM_{predicted} $$ \n",
"The implementation transforms the measurement and bias to the body frame first for efficiency if `body_P_sensor` is provided.\n",
"The error $e$ is calculated in the body frame."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Template Parameter**: `POSE` (must be `gtsam.Pose2` or `gtsam.Pose3`).\n",
"- **Constructor**: `MagPoseFactor(poseKey, measured, scale, direction, bias, model, body_P_sensor=None)`\n",
" - `poseKey`: Key of the `Pose2` or `Pose3` variable.\n",
" - `measured`: Measured magnetic field vector (`Point2` or `Point3`) **in the sensor frame**.\n",
" - `scale`: Known scale factor.\n",
" - `direction`: Known magnetic field direction (`Point2` or `Point3`) **in the navigation frame**.\n",
" - `bias`: Known bias vector (`Point2` or `Point3`) **in the sensor frame**.\n",
" - `model`: Noise model (2D or 3D).\n",
" - `body_P_sensor`: Optional `Pose2` or `Pose3` describing the sensor's pose relative to the body frame.\n",
"- **`evaluateError(nPb)`**: Calculates the error vector (2D or 3D) based on the current pose estimate `nPb`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example (Pose3)\n",
"\n",
"Using a magnetometer to help estimate a `Pose3` orientation, assuming known calibration and a sensor offset."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created MagPoseFactor<Pose3>:\n",
" keys = { x0 }\n",
"isotropic dim=3 sigma=50\n",
"local field (nM): [35176.3235; 5025.18908; 35176.3235];\n",
"measured field (bM): [28523.6117; 5040.18908; 40745.2206];\n",
"magnetometer bias: [-10; 15; -5];\n",
"\n",
"Error at ground truth pose (should be zero): [0. 0. 0.]\n",
"Error at test pose: [-3660.19475195 0. 2331.80122523]\n"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam.symbol_shorthand import X\n",
"\n",
"# --- Assumed Known Calibration & Field (NED Frame) ---\n",
"n_direction_point = gtsam.Point3(0.7, 0.1, 0.7) # Example simplified direction in NED\n",
"mag_scale = 50000.0 # nT\n",
"mag_bias_sensor = gtsam.Point3(15.0, 10.0, -5.0) # Bias in sensor frame (nT)\n",
"\n",
"# --- Sensor Pose --- \n",
"# Assume sensor is rotated 90 deg yaw right w.r.t body\n",
"body_P_sensor = gtsam.Pose3(gtsam.Rot3.Yaw(np.deg2rad(90)), gtsam.Point3(0.1, 0, 0))\n",
"sensor_P_body = body_P_sensor.inverse()\n",
"\n",
"# --- Simulation: Generate Measurement ---\n",
"# Assume a ground truth body pose (e.g., 10 deg pitch up in NED)\n",
"truth_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(10)), gtsam.Point3(1,2,3))\n",
"truth_nRb = truth_nPb.rotation()\n",
"truth_bRn = truth_nRb.inverse()\n",
"\n",
"# Calculate field in nav frame\n",
"n_field_vector = mag_scale * (n_direction_point / np.linalg.norm(n_direction_point))\n",
"\n",
"# Calculate field in sensor frame \n",
"truth_nRs = truth_nRb * body_P_sensor.rotation()\n",
"truth_sRn = truth_nRs.inverse()\n",
"s_field_ideal = truth_sRn.rotate(n_field_vector)\n",
"\n",
"# Calculate the measured value including bias (in sensor frame)\n",
"s_measured = s_field_ideal + mag_bias_sensor\n",
"\n",
"# --- Factor Creation ---\n",
"pose_key = X(0)\n",
"\n",
"# Noise model for the magnetometer measurement (nT)\n",
"mag_noise_sigma = 50.0 # nT\n",
"noise_model = gtsam.noiseModel.Isotropic.Sigma(3, mag_noise_sigma)\n",
"\n",
"# Create MagPoseFactor (providing body_P_sensor)\n",
"# Note: measurement and bias are in SENSOR frame when body_P_sensor is specified\n",
"mag_factor = gtsam.MagPoseFactorPose3(pose_key, s_measured, mag_scale, \n",
" n_direction_point, mag_bias_sensor, \n",
" noise_model, body_P_sensor)\n",
"\n",
"print(\"Created MagPoseFactor<Pose3>:\")\n",
"mag_factor.print()\n",
"\n",
"# --- Evaluate Error ---\n",
"# Evaluate at the ground truth pose (error should be zero)\n",
"error_at_truth = mag_factor.evaluateError(truth_nPb)\n",
"print(\"\\nError at ground truth pose (should be zero):\", error_at_truth)\n",
"\n",
"# Evaluate at a different pose (error should be non-zero)\n",
"test_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(15)), gtsam.Point3(1,2,3))\n",
"error_at_test = mag_factor.evaluateError(test_nPb)\n",
"print(\"Error at test pose:\", error_at_test)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Important Considerations\n",
"- **Frame Consistency**: Ensure `direction` is in the nav frame, while `measured` and `bias` are in the **sensor frame** when `body_P_sensor` is provided. If `body_P_sensor` is `None`, then `measured` and `bias` are assumed to be in the body frame.\n",
"- **Units**: Maintain consistent units (e.g., nT) for scale, bias, measurement, and noise sigma."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [MagPoseFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/MagPoseFactor.h)"
]
}
],
"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
}

188
gtsam/navigation/doc/PreintegratedCombinedMeasurements.ipynb Normal file
View File

@ -0,0 +1,188 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# PreintegratedCombinedMeasurements\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegratedCombinedMeasurements.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 `PreintegratedCombinedMeasurements` class is the preintegration container specifically designed for use with the `CombinedImuFactor`. Like `PreintegratedImuMeasurements`, it accumulates IMU measurements between two time steps ($t_i, t_j$) using a fixed bias estimate (`biasHat_`).\n",
"\n",
"However, it differs significantly in its covariance propagation:\n",
"1. It uses `PreintegrationCombinedParams`, which include bias random walk parameters.\n",
"2. It propagates a larger **15x15 covariance matrix** (`preintMeasCov_`). This matrix captures the uncertainty of the preintegrated $\\Delta R, \\Delta p, \\Delta v$ (9x9 block), the uncertainty of the bias estimate itself (6x6 block), and crucially, the **cross-correlations** between the preintegrated measurements and the bias estimate.\n",
"\n",
"Accounting for these correlations and the bias evolution noise allows the `CombinedImuFactor` to properly model the relationship between the states at $t_i, t_j$ and the biases at $b_i, b_j$."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n",
"\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n",
"\u001b[0mNote: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Background\n",
"\n",
"The mean propagation (calculating $\\Delta R, \\Delta p, \\Delta v$) is mathematically identical to that in `PreintegratedImuMeasurements`, using the same underlying implementation (`ManifoldPreintegration` or `TangentPreintegration`).\n",
"\n",
"The key difference lies in the covariance propagation. The update step for the 15x15 covariance matrix $\\Sigma_k \\rightarrow \\Sigma_{k+1}$ incorporates not only the IMU measurement noise (like the standard PIM) but also:\n",
"- The noise from the bias random walk process (defined in `PreintegrationCombinedParams`).\n",
"- The uncertainty of the `biasHat_` used during the integration step (via `biasAccOmegaInit` from the parameters).\n",
"\n",
"This results in a more complex but statistically more accurate propagation of uncertainty, especially when biases are expected to drift significantly or have substantial initial uncertainty. The derivation details can be found in technical reports and papers related to the `CombinedImuFactor` (e.g., Carlone et al., IJRR 2015)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"The API is very similar to `PreintegratedImuMeasurements`:\n",
"\n",
"- **Constructor**: `PreintegratedCombinedMeasurements(params, biasHat)`: Requires shared `PreintegrationCombinedParams`.\n",
"- **`integrateMeasurement(measuredAcc, measuredOmega, dt)`**: Adds a measurement, updating the internal state and the 15x15 covariance.\n",
"- **`resetIntegration()` / `resetIntegrationAndSetBias(newBiasHat)`**: Resets the integration.\n",
"- **Accessors**: `deltaTij()`, `deltaRij()`, `deltaPij()`, `deltaVij()`, `biasHat()`. \n",
"- **`preintMeasCov()`**: Returns the **15x15** covariance matrix.\n",
"- **`predict(state_i, current_bias)`**: Predicts state $X_j$ (same logic as standard PIM, using only the $\\Delta R, p, v$ parts).\n",
"- **`biasCorrectedDelta(current_bias)`**: Returns the 9D tangent-space vector corrected for bias difference (same logic as standard PIM).\n",
"- **Note**: There isn't a direct equivalent of `computeError` within this class, as the full error calculation (including the bias random walk part) is handled by the `CombinedImuFactor` itself."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Create combined parameters, create the object, integrate measurements."
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Total integration time: 0.09999999999999999\n",
"Delta R:\n",
" [[ 0.9999875 -0.00499998 0. ]\n",
" [ 0.00499998 0.9999875 0. ]\n",
" [ 0. 0. 1. ]]\n",
"Delta P: [ 4.99999147e-04 7.12499187e-07 -4.90000000e-02]\n",
"Delta V: [ 9.99996438e-03 2.24999578e-05 -9.80000000e-01]\n",
"Preintegration Covariance (15x15 shape): (15, 15)\n"
]
}
],
"source": [
"from gtsam import PreintegrationCombinedParams, PreintegratedCombinedMeasurements\n",
"from gtsam.imuBias import ConstantBias\n",
"import numpy as np\n",
"\n",
"# 1. Create Combined Parameters (as in PreintegrationCombinedParams example)\n",
"params = PreintegrationCombinedParams.MakeSharedU(9.81)\n",
"accel_noise_sigma = 0.1\n",
"gyro_noise_sigma = 0.01\n",
"params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
"params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
"params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
"bias_acc_rw_sigma = 0.001\n",
"bias_gyro_rw_sigma = 0.0001\n",
"params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)\n",
"params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)\n",
"initial_bias_cov = np.diag(np.full(6, 1e-3)) # Example initial bias uncertainty\n",
"params.setBiasAccOmegaInit(initial_bias_cov)\n",
"\n",
"# 2. Define the bias estimate used for preintegration\n",
"bias_hat = ConstantBias() # Start with zero bias estimate\n",
"\n",
"# 3. Create the PreintegratedCombinedMeasurements object\n",
"pim = PreintegratedCombinedMeasurements(params, bias_hat)\n",
"\n",
"# 4. Integrate measurements\n",
"dt = 0.01 # 100 Hz\n",
"num_measurements = 10\n",
"acc_meas = np.array([0.1, 0.0, -9.8]) \n",
"gyro_meas = np.array([0.0, 0.0, 0.05])\n",
"\n",
"for _ in range(num_measurements):\n",
" pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
"\n",
"# 5. Inspect the results\n",
"print(\"Total integration time:\", pim.deltaTij())\n",
"print(\"Delta R:\\n\", pim.deltaRij().matrix())\n",
"print(\"Delta P:\", pim.deltaPij())\n",
"print(\"Delta V:\", pim.deltaVij())\n",
"print(\"Preintegration Covariance (15x15 shape):\", pim.preintMeasCov().shape)\n",
"# print(\"Preintegration Covariance:\\n\", pim.preintMeasCov()) # Might be large"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This `pim` object is then passed to the `CombinedImuFactor` constructor."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [CombinedImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.h) (Contains definition)\n",
"- [CombinedImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.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
}

210
gtsam/navigation/doc/PreintegratedImuMeasurements.ipynb Normal file
View File

@ -0,0 +1,210 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# PreintegratedImuMeasurements\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegratedImuMeasurements.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 `PreintegratedImuMeasurements` class (often abbreviated as PIM) is the standard container in GTSAM for accumulating high-rate IMU measurements (accelerometer and gyroscope) between two keyframes or time steps ($t_i$ and $t_j$). It performs *preintegration*, effectively summarizing the relative motion $(\\Delta R_{ij}, \\Delta p_{ij}, \\Delta v_{ij})$ predicted by the IMU measurements, while accounting for sensor noise and a *fixed estimate* of the IMU bias (`biasHat_`).\n",
"\n",
"This preintegrated summary allows IMU information to be incorporated into a factor graph as a single factor (`ImuFactor` or `ImuFactor2`) between states at $t_i$ and $t_j$, avoiding the need to add every single IMU measurement to the graph. It also stores the 9x9 covariance matrix (`preintMeasCov_`) associated with the uncertainty of the preintegrated state change.\n",
"\n",
"It inherits from an underlying implementation (`ManifoldPreintegration` or `TangentPreintegration`, selected at compile time) which defines the specific mathematical approach used for integration."
]
},
{
"cell_type": "code",
"execution_count": null,
"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-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Background\n",
"\n",
"The core idea is to integrate the IMU kinematic equations *relative* to the state at time $t_i$, using the bias estimate `biasHat_` held within the class. The exact integration formulas depend on the chosen implementation (manifold or tangent space), but conceptually:\n",
"\n",
"$$ \\Delta R_{ij} = \\prod_{k=i}^{j-1} \\text{Exp}((\\omega_k - b_{g, est}) \\Delta t) $$ \n",
"$$ \\Delta v_{ij} = \\sum_{k=i}^{j-1} \\Delta R_{ik} (a_k - b_{a, est}) \\Delta t $$ \n",
"$$ \\Delta p_{ij} = \\sum_{k=i}^{j-1} [ \\Delta v_{ik} \\Delta t + \\frac{1}{2} \\Delta R_{ik} (a_k - b_{a, est}) \\Delta t^2 ] $$ \n",
"\n",
"where $\\omega_k$ and $a_k$ are the measurements at step k, $b_{g, est}$ and $b_{a, est}$ are the components of `biasHat_`, and $\\Delta R_{ik}$ is the preintegrated rotation from $i$ to $k$.\n",
"\n",
"Crucially, the class also propagates the covariance of these $\\Delta$ terms based on the IMU noise specified in `PreintegrationParams`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Constructor**: `PreintegratedImuMeasurements(params, biasHat)`: Creates an instance, requiring shared `PreintegrationParams` and the initial bias estimate (`biasHat_`) used for integration.\n",
"- **`integrateMeasurement(measuredAcc, measuredOmega, dt)`**: Adds a single IMU measurement pair and time delta, updating the internal preintegrated state and covariance.\n",
"- **`resetIntegration()`**: Resets the accumulated measurements and time to zero, keeping the `biasHat_`.\n",
"- **`resetIntegrationAndSetBias(newBiasHat)`**: Resets integration and updates the internal `biasHat_`.\n",
"- **Accessors**: `deltaTij()`, `deltaRij()`, `deltaPij()`, `deltaVij()`, `preintMeasCov()`, `biasHat()`. These return the accumulated values.\n",
"- **`predict(state_i, current_bias)`**: Predicts the state at time $t_j$ given the state at $t_i$ and the *current best estimate* of the bias (which might differ from `biasHat_`). This function applies the necessary first-order corrections for the change in bias.\n",
"- **`biasCorrectedDelta(current_bias)`**: Returns the 9D tangent-space representation of the preintegrated measurement, corrected for the difference between `current_bias` and `biasHat_`.\n",
"- **`computeError(state_i, state_j, current_bias)`**: Calculates the 9D error between the preintegrated prediction (using `current_bias`) and the actual state change (`state_i` to `state_j`). This is the core calculation used within `ImuFactor::evaluateError`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Create parameters, create the PIM object with an initial bias estimate, integrate measurements, and potentially use `predict`."
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Total integration time: 0.09999999999999999\n",
"Delta R:\n",
" [[ 9.99992775e-01 -3.10098211e-03 -2.19859941e-03]\n",
" [ 3.09900212e-03 9.99994790e-01 -9.03407707e-04]\n",
" [ 2.20138940e-03 8.96587715e-04 9.99997175e-01]]\n",
"Delta P: [ 0.00047953 0.00106289 -0.04859943]\n",
"Delta V: [ 0.00993257 0.02140713 -0.97198182]\n",
"Bias Hat Used:\n",
"acc = 0.01 -0.01 0.02 gyro = 0.001 0.002 -0.001\n",
"Preintegration Covariance (9x9):\n",
" [[ 0.01 0. -0. -0. 0. 0. -0. 0. 0. ]\n",
" [ 0. 0.01 0. -0. -0. -0. -0. -0. -0. ]\n",
" [-0. 0. 0.01 -0. 0. -0. -0. 0. -0. ]\n",
" [-0. -0. -0. 0. -0. 0. 0.05 -0. 0. ]\n",
" [ 0. -0. 0. -0. 0. 0. -0. 0.05 0. ]\n",
" [ 0. -0. -0. 0. 0. 0. 0. 0. 0.05]\n",
" [-0. -0. -0. 0.05 -0. 0. 1. -0. 0. ]\n",
" [ 0. -0. 0. -0. 0.05 0. -0. 1. 0. ]\n",
" [ 0. -0. -0. 0. 0. 0.05 0. 0. 1. ]]\n",
"\n",
"Predicted State:\n",
"R: [\n",
"\t0.999993, -0.00310098, -0.0021986;\n",
"\t0.003099, 0.999995, -0.000903408;\n",
"\t0.00220139, 0.000896588, 0.999997\n",
"]\n",
"p: 0.000479535 0.00106289 -0.0976494\n",
"v: 0.00993257 0.0214071 -1.95298\n"
]
}
],
"source": [
"from gtsam import PreintegrationParams, PreintegratedImuMeasurements, NavState\n",
"from gtsam.imuBias import ConstantBias\n",
"import numpy as np\n",
"\n",
"# 1. Create Parameters (as in PreintegrationParams example)\n",
"params = PreintegrationParams.MakeSharedU(9.81)\n",
"accel_noise_sigma = 0.1\n",
"gyro_noise_sigma = 0.01\n",
"params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
"params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
"params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
"\n",
"# 2. Define the bias estimate used for preintegration\n",
"initial_bias_acc = np.array([0.01, -0.01, 0.02])\n",
"initial_bias_gyro = np.array([0.001, 0.002, -0.001])\n",
"bias_hat = ConstantBias(initial_bias_acc, initial_bias_gyro)\n",
"\n",
"# 3. Create the PreintegratedImuMeasurements object\n",
"pim = PreintegratedImuMeasurements(params, bias_hat)\n",
"\n",
"# 4. Integrate measurements (example loop)\n",
"dt = 0.01 # 100 Hz\n",
"num_measurements = 10\n",
"acc_meas = np.array([0.1, 0.2, -9.7]) # Example measurement (sensor frame)\n",
"gyro_meas = np.array([0.01, -0.02, 0.03]) # Example measurement (sensor frame)\n",
"\n",
"for _ in range(num_measurements):\n",
" pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
"\n",
"# 5. Inspect the results\n",
"print(\"Total integration time:\", pim.deltaTij())\n",
"print(\"Delta R:\\n\", pim.deltaRij().matrix())\n",
"print(\"Delta P:\", pim.deltaPij())\n",
"print(\"Delta V:\", pim.deltaVij())\n",
"print(\"Bias Hat Used:\")\n",
"pim.biasHat().print()\n",
"print(\"Preintegration Covariance (9x9):\\n\", np.round(1000*pim.preintMeasCov(),2))\n",
"\n",
"# 6. Example Prediction (requires initial state)\n",
"initial_state = NavState() # Default: Identity rotation, zero pos/vel\n",
"current_best_bias = bias_hat # Assume bias estimate hasn't changed yet\n",
"predicted_state = pim.predict(initial_state, current_best_bias)\n",
"print(\"\\nPredicted State:\")\n",
"predicted_state.print()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The `pim` object is then passed to the `ImuFactor` or `ImuFactor2` constructor."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [ImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.h) (Contains PIM definition)\n",
"- [ImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.cpp)\n",
"- [ManifoldPreintegration.h/.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldPreintegration.h) (One possible implementation)\n",
"- [TangentPreintegration.h/.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/TangentPreintegration.h) (Another possible implementation)"
]
}
],
"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
}

4
gtsam/navigation/doc/PreintegratedRotation.ipynb
View File

@ -20,7 +20,7 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
@ -36,7 +36,7 @@
}
],
"source": [
"%pip install --quiet gtsam plotly"
"%pip install --quiet gtsam-develop plotly"
]
},
{

214
gtsam/navigation/doc/PreintegrationCombinedParams.ipynb Normal file
View File

@ -0,0 +1,214 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# PreintegrationCombinedParams\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegrationCombinedParams.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 `PreintegrationCombinedParams` class holds parameters specifically required for the `CombinedImuFactor` and its associated `PreintegratedCombinedMeasurements` class. \n",
"\n",
"It inherits from `PreintegrationParams` (and thus also `PreintegratedRotationParams`) and adds parameters that model the *evolution* of the IMU bias over time, typically as a random walk process. This is essential for the `CombinedImuFactor`, which estimates biases at both the start ($b_i$) and end ($b_j$) of the preintegration interval."
]
},
{
"cell_type": "code",
"execution_count": null,
"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-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Parameters\n",
"\n",
"In addition to all parameters from `PreintegrationParams`, this class adds:\n",
"\n",
"- **`biasAccCovariance`**: A 3x3 matrix representing the continuous-time covariance of the *random walk process* driving the accelerometer bias. Units: (m²/s⁵)/Hz ? (Check docs, represents variance growth rate).\n",
"- **`biasOmegaCovariance`**: A 3x3 matrix representing the continuous-time covariance of the *random walk process* driving the gyroscope bias. Units: (rad²/s³)/Hz ? (Check docs, represents variance growth rate).\n",
"- **`biasAccOmegaInit`**: A 6x6 matrix representing the covariance of the uncertainty in the *initial* bias estimate provided to the preintegration. This accounts for the fact that the bias used for integration (`biasHat_`) is itself uncertain."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Mathematical Background: Bias Random Walk\n",
"\n",
"The `CombinedImuFactor` implicitly assumes that the bias evolves between time steps according to a random walk:\n",
"$$ b_{k+1} = b_k + w_k, \\quad w_k \\sim \\mathcal{N}(0, Q_b \\Delta t) $$ \n",
"where $b_k = [b_{a,k}; b_{g,k}]$ is the 6D bias vector at time $k$, $w_k$ is zero-mean Gaussian noise, and $Q_b$ is the block-diagonal continuous-time covariance matrix formed from `biasAccCovariance` and `biasOmegaCovariance`:\n",
"$$ Q_b = \\begin{bmatrix} \\text{biasAccCovariance} & 0 \\\\ 0 & \\text{biasOmegaCovariance} \\end{bmatrix} $$ \n",
"The factor uses this model (integrated over the interval $\\Delta t_{ij}$) to constrain the difference between $b_i$ and $b_j$."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Constructors**: \n",
" - `PreintegrationCombinedParams(n_gravity)`: Main constructor.\n",
" - `MakeSharedD(g=9.81)` / `MakeSharedU(g=9.81)`: Convenience static methods for NED/ENU frames.\n",
"- **Setters**: Methods like `setBiasAccCovariance`, `setBiasOmegaCovariance`, `setBiasAccOmegaInit`, plus all setters inherited from `PreintegrationParams`.\n",
"- **Getters**: Corresponding getters for the combined parameters, plus all inherited getters.\n",
"- **`print` / `equals`**: Standard methods."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Create parameters, often starting from the base `PreintegrationParams` settings, and then set the bias evolution parameters."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Combined IMU Preintegration Parameters:\n",
"\n",
"gyroscopeCovariance:\n",
"[\n",
"0.0001 0 0\n",
" 0 0.0001 0\n",
" 0 0 0.0001\n",
"]\n",
"accelerometerCovariance:\n",
"[\n",
"0.01 0 0\n",
" 0 0.01 0\n",
" 0 0 0.01\n",
"]\n",
"integrationCovariance:\n",
"[\n",
"1e-08 0 0\n",
" 0 1e-08 0\n",
" 0 0 1e-08\n",
"]\n",
"n_gravity = ( 0 0 -9.81)\n",
"biasAccCovariance:\n",
"[\n",
"1e-06 0 0\n",
" 0 1e-06 0\n",
" 0 0 1e-06\n",
"]\n",
"biasOmegaCovariance:\n",
"[\n",
"1e-08 0 0\n",
" 0 1e-08 0\n",
" 0 0 1e-08\n",
"]\n",
"biasAccOmegaInt:\n",
"[\n",
" 0.01 0 0 0 0 0\n",
" 0 0.01 0 0 0 0\n",
" 0 0 0.01 0 0 0\n",
" 0 0 0 0.0025 0 0\n",
" 0 0 0 0 0.0025 0\n",
" 0 0 0 0 0 0.0025\n",
"]\n"
]
}
],
"source": [
"from gtsam import PreintegrationCombinedParams\n",
"import numpy as np\n",
"\n",
"# 1. Create parameters for an ENU navigation frame (Z-up)\n",
"params = PreintegrationCombinedParams.MakeSharedU(9.81)\n",
"\n",
"# 2. Set standard noise parameters (accel, gyro, integration)\n",
"accel_noise_sigma = 0.1\n",
"gyro_noise_sigma = 0.01\n",
"params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
"params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
"params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
"\n",
"# 3. Set bias random walk noise parameters (example values)\n",
"bias_acc_rw_sigma = 0.001 # m/s^2 / sqrt(s) -> Covariance = sigma^2\n",
"bias_gyro_rw_sigma = 0.0001 # rad/s / sqrt(s) -> Covariance = sigma^2\n",
"params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)\n",
"params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)\n",
"\n",
"# 4. Set initial bias uncertainty (covariance of bias used for preintegration)\n",
"# Example: Assume bias estimate used for preintegration has some uncertainty\n",
"initial_bias_acc_sigma = 0.1\n",
"initial_bias_gyro_sigma = 0.05\n",
"initial_bias_cov = np.diag(np.concatenate([\n",
" np.full(3, initial_bias_acc_sigma**2),\n",
" np.full(3, initial_bias_gyro_sigma**2)\n",
"]))\n",
"params.setBiasAccOmegaInit(initial_bias_cov)\n",
"\n",
"print(\"Combined IMU Preintegration Parameters:\")\n",
"params.print()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"These combined parameters are then passed to `PreintegratedCombinedMeasurements`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [PreintegrationCombinedParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationCombinedParams.h)\n",
"- [CombinedImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.cpp) (Contains the definition of this class)"
]
}
],
"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
}

169
gtsam/navigation/doc/PreintegrationParams.ipynb
View File

@ -6,48 +6,143 @@
"source": [
"# PreintegrationParams\n",
"\n",
"The `PreintegrationParams` class extends `PreintegratedRotationParams` to provide a complete configuration for IMU preintegration, including accelerometer measurements:\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegrationParams.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\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",
"## Overview\n",
"\n",
"## Convenience API\n",
"The `PreintegrationParams` class holds the parameters required for Inertial Measurement Unit (IMU) preintegration in GTSAM. It configures how IMU measurements (accelerometer and gyroscope) are handled, including noise characteristics, gravity direction, and Coriolis effects.\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",
"This class extends `PreintegratedRotationParams` (which handles gyroscope-specific parameters) by adding parameters relevant to accelerometer measurements and the combined integration process.\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$."
"A single `PreintegrationParams` object (usually created as a `shared_ptr`) is typically shared among multiple preintegration instances (`PreintegratedImuMeasurements` or `PreintegratedCombinedMeasurements`)."
]
},
{
"cell_type": "code",
"execution_count": null,
"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-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Detailed API\n",
"## Key Parameters\n",
"\n",
"- The constructor requires the navigation frame gravity vector as an argument.\n",
"In addition to parameters inherited from `PreintegratedRotationParams` (like `gyroscopeCovariance`, `omegaCoriolis`, `body_P_sensor`), `PreintegrationParams` includes:\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",
"- **`accelerometerCovariance`**: A 3x3 matrix representing the continuous-time noise covariance of the accelerometer. Units: (m/s²)²/Hz.\n",
"- **`integrationCovariance`**: A 3x3 matrix representing additional uncertainty introduced during the numerical integration process itself (often set to zero). Units: (m²/s⁶)/Hz ? (Consult documentation for precise interpretation).\n",
"- **`use2ndOrderCoriolis`**: A boolean flag. If true, enables a more accurate (second-order) correction for Coriolis effects, which arise from the Earth's rotation.\n",
"- **`n_gravity`**: A 3D vector specifying the direction and magnitude of gravity in the navigation frame (e.g., [0, 0, -9.81] for ENU, [0, 0, 9.81] for NED). Units: m/s²."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\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."
"- **Constructors**: \n",
" - `PreintegrationParams(n_gravity)`: Main constructor requiring the gravity vector.\n",
" - `MakeSharedD(g=9.81)`: Convenience static method to create shared parameters for a Z-down (NED) navigation frame.\n",
" - `MakeSharedU(g=9.81)`: Convenience static method to create shared parameters for a Z-up (ENU) navigation frame.\n",
"- **Setters**: Methods like `setAccelerometerCovariance`, `setIntegrationCovariance`, `setUse2ndOrderCoriolis`, `setGyroscopeCovariance`, `setOmegaCoriolis`, `setBodyPSensor` allow configuring the parameters after creation.\n",
"- **Getters**: Corresponding methods like `getAccelerometerCovariance`, `getIntegrationCovariance`, `getGravity`, `isUsing2ndOrderCoriolis` allow retrieving parameter values.\n",
"- **`print` / `equals`**: Standard methods for displaying parameters and comparing them."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Typically, you create parameters once using a convenience function and then adjust specific noise values."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"IMU Preintegration Parameters:\n",
"\n",
"gyroscopeCovariance:\n",
"[\n",
"1e-06 0 0\n",
" 0 1e-06 0\n",
" 0 0 1e-06\n",
"]\n",
"accelerometerCovariance:\n",
"[\n",
"0.0001 0 0\n",
" 0 0.0001 0\n",
" 0 0 0.0001\n",
"]\n",
"integrationCovariance:\n",
"[\n",
"1e-08 0 0\n",
" 0 1e-08 0\n",
" 0 0 1e-08\n",
"]\n",
"n_gravity = ( 0 0 -9.81)\n"
]
}
],
"source": [
"from gtsam import PreintegrationParams\n",
"import numpy as np\n",
"\n",
"# Create parameters for an ENU navigation frame (Z-up)\n",
"# Using standard gravity, g=9.81 m/s^2\n",
"params = PreintegrationParams.MakeSharedU(9.81)\n",
"\n",
"# Set accelerometer noise characteristics (example values)\n",
"accel_noise_sigma = 0.01 # m/s^2 / sqrt(Hz)\n",
"params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
"\n",
"# Set gyroscope noise characteristics (example values)\n",
"gyro_noise_sigma = 0.001 # rad/s / sqrt(Hz)\n",
"params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
"\n",
"# Set integration uncertainty (often zero)\n",
"params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
"\n",
"# Optionally, set sensor pose relative to body\n",
"# params.setBodyPSensor(gtsam.Pose3(...))\n",
"\n",
"# Optionally, enable 2nd order Coriolis correction\n",
"# params.setUse2ndOrderCoriolis(True)\n",
"\n",
"print(\"IMU Preintegration Parameters:\")\n",
"params.print()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"These parameters are then passed to `PreintegratedImuMeasurements` or `PreintegratedCombinedMeasurements` when they are constructed."
]
},
{
@ -61,8 +156,22 @@
}
],
"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,

196
gtsam/navigation/doc/Scenario.ipynb Normal file
View File

@ -0,0 +1,196 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Scenario Class Hierarchy\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/Scenario.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 `Scenario` class hierarchy provides a simple way to define theoretical trajectories for testing navigation algorithms and factors, particularly IMU factors. \n",
"\n",
"The base `Scenario` class is abstract and defines an interface that requires subclasses to provide the ground truth pose, angular velocity (in body frame), linear velocity (in nav frame), and linear acceleration (in nav frame) at any given continuous time `t`.\n",
"\n",
"Concrete subclasses like `ConstantTwistScenario` and `AcceleratingScenario` implement specific types of motion."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Base Class: `Scenario`\n",
"\n",
"Defines the required interface for any trajectory scenario.\n",
"\n",
"**Pure Virtual Methods (must be implemented by subclasses):**\n",
"- `pose(t)`: Returns the `Pose3` of the body in the navigation frame at time `t`.\n",
"- `omega_b(t)`: Returns the angular velocity `Vector3` in the **body frame** at time `t`.\n",
"- `velocity_n(t)`: Returns the linear velocity `Vector3` in the **navigation frame** at time `t`.\n",
"- `acceleration_n(t)`: Returns the linear acceleration `Vector3` in the **navigation frame** at time `t`.\n",
"\n",
"**Derived Methods (provided by the base class):**\n",
"- `rotation(t)`: Returns the `Rot3` part of `pose(t)`.\n",
"- `navState(t)`: Returns the `NavState` (Pose + Nav Velocity) at time `t`.\n",
"- `velocity_b(t)`: Calculates linear velocity in the **body frame** using $v_b = R_{bn} v_n$.\n",
"- `acceleration_b(t)`: Calculates linear acceleration in the **body frame** using $a_b = R_{bn} a_n$."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Concrete Scenarios\n",
"\n",
"### `ConstantTwistScenario`\n",
"\n",
"- **Description**: Models motion with a constant *twist* (angular velocity $\\omega_b$ and linear velocity $v_b$) defined in the **body frame**.\n",
"- **Motion**: Results in helical or circular motion trajectories.\n",
"- **Constructors**:\n",
" - `ConstantTwistScenario(omega_b, velocity_b, initial_nTb=Pose3())`\n",
" - `ConstantTwistScenario(twist_vector_6d, initial_nTb=Pose3())`\n",
"- **Key Behavior**: `omega_b(t)` is constant. `velocity_n(t)` and `acceleration_n(t)` are calculated based on the constant body-frame velocities and the changing orientation.\n",
"\n",
"### `AcceleratingScenario`\n",
"\n",
"- **Description**: Models motion with constant linear acceleration $a_n$ defined in the **navigation frame** and constant angular velocity $\\omega_b$ defined in the **body frame**.\n",
"- **Motion**: Represents scenarios like accelerating along a straight line while potentially rotating (e.g., aircraft takeoff, car acceleration).\n",
"- **Constructor**: `AcceleratingScenario(initial_nRb, initial_p_n, initial_v_n, const_a_n, const_omega_b=zero)`\n",
"- **Key Behavior**: `acceleration_n(t)` and `omega_b(t)` are constant. `velocity_n(t)` and `pose(t)` are integrated from the initial conditions and constant rates."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Instantiate a scenario and query its properties at different times."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"--- Circle Scenario ---\n",
"Pose at t=1.0:\n",
"R: [\n",
"\t0.995004, -0.0998334, 0;\n",
"\t0.0998334, 0.995004, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 1.99667 0.0999167 0\n",
"Nav Velocity at t=1.0: [1.99000833 0.19966683 0. ]\n",
"Body Omega at t=1.0: [0. 0. 0.1]\n",
"Nav Acceleration at t=1.0: [-0.01996668 0.19900083 0. ]\n",
"\n",
"--- Accelerating Scenario ---\n",
"Pose at t=2.0:\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 1 0 0\n",
"Nav Velocity at t=2.0: [1. 0. 0.]\n",
"Nav Acceleration at t=2.0: [0.5 0. 0. ]\n"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"\n",
"# --- ConstantTwistScenario Example: Moving in a Circle --- \n",
"# Forward velocity 2 m/s, turning left (positive Z rot) at ~5.73 deg/s (0.1 rad/s)\n",
"omega_b_circle = np.array([0.0, 0.0, 0.1]) \n",
"vel_b_circle = np.array([2.0, 0.0, 0.0]) # Forward velocity along body X\n",
"initial_pose_circle = gtsam.Pose3() # Start at origin\n",
"\n",
"circle_scenario = gtsam.ConstantTwistScenario(omega_b_circle, vel_b_circle, initial_pose_circle)\n",
"\n",
"print(\"--- Circle Scenario ---\")\n",
"time_t = 1.0 # seconds\n",
"print(f\"Pose at t={time_t}:\")\n",
"circle_scenario.pose(time_t).print()\n",
"print(f\"Nav Velocity at t={time_t}: {circle_scenario.velocity_n(time_t)}\")\n",
"print(f\"Body Omega at t={time_t}: {circle_scenario.omega_b(time_t)}\")\n",
"print(f\"Nav Acceleration at t={time_t}: {circle_scenario.acceleration_n(time_t)}\") # Centripetal\n",
"\n",
"# --- AcceleratingScenario Example: Accelerating Straight --- \n",
"initial_pose_accel = gtsam.Pose3() \n",
"initial_vel_n = np.array([0.0, 0.0, 0.0])\n",
"const_accel_n = np.array([0.5, 0.0, 0.0]) # Accelerate along nav X\n",
"const_omega_b = np.array([0.0, 0.0, 0.0]) # No rotation\n",
"\n",
"accel_scenario = gtsam.AcceleratingScenario(\n",
" initial_pose_accel.rotation(), initial_pose_accel.translation(),\n",
" initial_vel_n, const_accel_n, const_omega_b\n",
")\n",
"\n",
"print(\"\\n--- Accelerating Scenario ---\")\n",
"time_t = 2.0 # seconds\n",
"print(f\"Pose at t={time_t}:\")\n",
"accel_scenario.pose(time_t).print()\n",
"print(f\"Nav Velocity at t={time_t}: {accel_scenario.velocity_n(time_t)}\")\n",
"print(f\"Nav Acceleration at t={time_t}: {accel_scenario.acceleration_n(time_t)}\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"These `Scenario` objects are primarily used as input to the `ScenarioRunner` class for generating simulated IMU data."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [Scenario.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)"
]
}
],
"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
}

224
gtsam/navigation/doc/ScenarioRunner.ipynb Normal file
View File

@ -0,0 +1,224 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# ScenarioRunner\n",
"\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/ScenarioRunner.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 `ScenarioRunner` class is a utility designed for testing IMU preintegration and factors. It takes a ground truth trajectory defined by a `Scenario` object, along with IMU parameters (`PreintegrationParams` or `PreintegrationCombinedParams`) and a specified IMU bias, and simulates the measurements an IMU would produce while following that trajectory.\n",
"\n",
"Key capabilities include:\n",
"- Calculating the ideal specific force and angular velocity at any time `t` based on the scenario's motion.\n",
"- Generating noisy IMU measurements by adding the specified bias and sampling from the noise models defined in the parameters.\n",
"- Integrating these simulated measurements over a time interval `T` to produce a `PreintegratedImuMeasurements` or `PreintegratedCombinedMeasurements` object.\n",
"- Predicting the final state based on an initial state and a preintegrated measurement object.\n",
"- Estimating the covariance of the preintegrated measurements or the prediction error via Monte Carlo simulation (useful for verifying the analytical covariance propagation)."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Measurement Simulation\n",
"\n",
"- **`actualAngularVelocity(t)`**: Returns the true angular velocity $\\omega_b(t)$ from the `Scenario`.\n",
"- **`actualSpecificForce(t)`**: Calculates the true specific force (what an ideal accelerometer measures) in the body frame. This is the body-frame acceleration $a_b(t)$ *minus* the body-frame representation of gravity $g_b(t)$:\n",
" $$ f_b(t) = a_b(t) - R_{bn}(t) g_n = R_{bn}(t) (a_n(t) - g_n) $$ \n",
" where $g_n$ is the gravity vector defined in `PreintegrationParams`.\n",
"- **`measuredAngularVelocity(t)`**: Adds bias and sampled noise to `actualAngularVelocity(t)`.\n",
" $$ \\omega_{measured} = \\omega_b(t) + b_g + \\eta_{gd} $$ \n",
" where $b_g$ is the gyro bias and $\\eta_{gd}$ is sampled discrete gyro noise.\n",
"- **`measuredSpecificForce(t)`**: Adds bias and sampled noise to `actualSpecificForce(t)`.\n",
" $$ a_{measured} = f_b(t) + b_a + \\eta_{ad} $$ \n",
" where $b_a$ is the accelerometer bias and $\\eta_{ad}$ is sampled discrete accelerometer noise."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Key Functionality / API\n",
"\n",
"- **Constructor**: `ScenarioRunner(scenario, params, imuSampleTime=0.01, bias=ConstantBias())`: Takes the scenario, IMU parameters, sample rate, and the *true* bias to apply to measurements.\n",
"- **Measurement Methods**: `actual...()`, `measured...()` as described above.\n",
"- **`integrate(T, estimatedBias, corrupted=True)`**: Simulates measurements at `imuSampleTime` intervals for a duration `T`, optionally adding noise (`corrupted=True`), and integrates them into a PIM object using `estimatedBias` as the `biasHat` for the PIM.\n",
"- **`predict(pim, estimatedBias)`**: Uses the provided PIM (which contains $\\Delta R, \\Delta p, \\Delta v$) and an `estimatedBias` to predict the final `NavState` starting from the scenario's initial state at $t=0$.\n",
"- **`estimateCovariance(T, N, estimatedBias)`**: Performs Monte Carlo simulation: runs `integrate` `N` times with different noise samples, calculates the `predict`ed state for each, and computes the sample covariance of the 9D tangent space difference between the Monte Carlo predictions and the mean prediction. Used to verify `pim.preintMeasCov()`.\n",
"- **`estimateNoiseCovariance(N)`**: Samples noise `N` times and computes the sample covariance, to verify the noise samplers themselves.\n",
"\n",
"There is also a `CombinedScenarioRunner` inheriting from `ScenarioRunner` that works with `PreintegrationCombinedParams` and `PreintegratedCombinedMeasurements`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Usage Example\n",
"\n",
"Using the `AcceleratingScenario` from the `Scenario` example to generate measurements and verify prediction."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"--- Integration Results (T=2.0s) ---\n",
"Bias Hat used in PIM:\n",
"acc = 0.04 -0.04 0.09 gyro = 0.005 -0.005 0.015\n",
"PIM Delta R:\n",
"R: [\n",
"\t0.9998, -0.00700878, -0.0187374;\n",
"\t0.00661627, 0.999759, -0.0209287;\n",
"\t0.0188796, 0.0208006, 0.999605\n",
"]\n",
"PIM Delta P: [ 0.70090612 -0.06192379 19.6232812 ]\n",
"PIM Delta V: [ 0.62536677 -0.17585342 19.47976488]\n",
"\n",
"--- Prediction vs Ground Truth (T=2.0s) ---\n",
"Predicted State:\n",
"R: [\n",
"\t0.9998, -0.00700878, -0.0187374;\n",
"\t0.00661627, 0.999759, -0.0209287;\n",
"\t0.0188796, 0.0208006, 0.999605\n",
"]\n",
"p: 0.700906 -0.0619238 0.0032812\n",
"v: 0.625367 -0.175853 -0.140235\n",
"\n",
"Ground Truth State:\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
"p: 1 0 0\n",
"v: 1 0 0\n",
"\n",
"Prediction Error (Logmap(predicted^-1 * ground_truth)):\n",
" [-0.02086757 0.01881111 -0.00681348 0.29938178 0.05974434 -0.01018013\n",
" 0.37836933 0.1761023 0.12947973]\n"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"\n",
"# --- 1. Define Scenario --- \n",
"initial_pose_accel = gtsam.Pose3() \n",
"initial_vel_n = np.array([0.0, 0.0, 0.0])\n",
"const_accel_n = np.array([0.5, 0.0, 0.0]) # Accelerate along nav X (ENU)\n",
"const_omega_b = np.array([0.0, 0.0, 0.0]) # No rotation\n",
"scenario = gtsam.AcceleratingScenario(\n",
" initial_pose_accel.rotation(), initial_pose_accel.translation(),\n",
" initial_vel_n, const_accel_n, const_omega_b\n",
")\n",
"\n",
"# --- 2. Define IMU Params and Runner --- \n",
"# Use default ENU parameters (Z-up, g=-9.81)\n",
"params = gtsam.PreintegrationParams.MakeSharedU(9.81)\n",
"# Add some noise (variances)\n",
"params.setAccelerometerCovariance(np.eye(3) * 0.01)\n",
"params.setGyroscopeCovariance(np.eye(3) * 0.0001)\n",
"params.setIntegrationCovariance(np.eye(3) * 1e-9)\n",
"\n",
"imu_sample_dt = 0.01 # 100 Hz\n",
"# Define the *true* bias applied to the measurements\n",
"true_bias = gtsam.imuBias.ConstantBias(np.array([0.05, -0.05, 0.1]), \n",
" np.array([0.01, -0.01, 0.02]))\n",
"\n",
"runner = gtsam.ScenarioRunner(scenario, params, imu_sample_dt, true_bias)\n",
"\n",
"# --- 3. Integrate Measurements --- \n",
"integration_time = 2.0 # seconds\n",
"\n",
"# Define the bias *estimate* to be used during preintegration\n",
"# Let's assume we have a slightly wrong estimate\n",
"estimated_bias = gtsam.imuBias.ConstantBias(np.array([0.04, -0.04, 0.09]), \n",
" np.array([0.005, -0.005, 0.015]))\n",
"\n",
"# Integrate noisy measurements using the 'estimated_bias' as biasHat for the PIM\n",
"pim = runner.integrate(integration_time, estimatedBias=estimated_bias, corrupted=True)\n",
"\n",
"print(f\"--- Integration Results (T={integration_time}s) ---\")\n",
"print(\"Bias Hat used in PIM:\")\n",
"pim.biasHat().print()\n",
"print(\"PIM Delta R:\")\n",
"pim.deltaRij().print()\n",
"print(\"PIM Delta P:\", pim.deltaPij())\n",
"print(\"PIM Delta V:\", pim.deltaVij())\n",
"\n",
"# --- 4. Predict State --- \n",
"# Predict the state at integration_time using the PIM and the *same* estimated_bias\n",
"initial_state_actual = scenario.navState(0.0)\n",
"predicted_state = runner.predict(pim, estimated_bias)\n",
"\n",
"# Get the ground truth state at the end time\n",
"ground_truth_state = scenario.navState(integration_time)\n",
"\n",
"print(f\"\\n--- Prediction vs Ground Truth (T={integration_time}s) ---\")\n",
"print(\"Predicted State:\")\n",
"predicted_state.print()\n",
"print(\"\\nGround Truth State:\")\n",
"ground_truth_state.print()\n",
"\n",
"# Calculate the error (difference in tangent space)\n",
"prediction_error = predicted_state.localCoordinates(ground_truth_state)\n",
"print(\"\\nPrediction Error (Logmap(predicted^-1 * ground_truth)):\\n\", prediction_error)\n",
"# Note: Error is non-zero due to noise and bias estimation error used in PIM.\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [ScenarioRunner.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ScenarioRunner.h)\n",
"- [ScenarioRunner.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ScenarioRunner.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
}

112
gtsam/navigation/navigation.i
View File

@ -346,6 +346,7 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor {
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nRef() const;
gtsam::Unit3 bMeasured() const;
gtsam::Vector evaluateError(const gtsam::Rot3& nRb);
// enable serialization functionality
void serialize() const;
@ -363,6 +364,7 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nRef() const;
gtsam::Unit3 bMeasured() const;
gtsam::Vector evaluateError(const gtsam::Pose3& nTb);
// enable serialization functionality
void serialize() const;
@ -380,6 +382,42 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Vector evaluateError(const gtsam::Pose3& nTb);
// enable serialization functionality
void serialize() const;
};
virtual class GPSFactorArm : gtsam::NonlinearFactor{
GPSFactorArm(size_t key, const gtsam::Point3& gpsIn,
const gtsam::Point3& leverArm,
const gtsam::noiseModel::Base* model);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol);
// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Vector evaluateError(const gtsam::Pose3& nTb);
// enable serialization functionality
void serialize() const;
};
virtual class GPSFactorArmCalib : gtsam::NonlinearFactor{
GPSFactorArmCalib(size_t key1, size_t key2, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol);
// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Vector evaluateError(const gtsam::Pose3& nTb, const gtsam::Point3& leverArm);
// enable serialization functionality
void serialize() const;
@ -396,6 +434,42 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Vector evaluateError(const gtsam::NavState& nTb);
// enable serialization functionality
void serialize() const;
};
virtual class GPSFactor2Arm : gtsam::NonlinearFactor{
GPSFactor2Arm(size_t key, const gtsam::Point3& gpsIn,
const gtsam::Point3& leverArm,
const gtsam::noiseModel::Base* model);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol);
// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Vector evaluateError(const gtsam::NavState& nTb);
// enable serialization functionality
void serialize() const;
};
virtual class GPSFactor2ArmCalib : gtsam::NonlinearFactor{
GPSFactor2ArmCalib(size_t key1, size_t key2, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol);
// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Vector evaluateError(const gtsam::NavState& nTb, const gtsam::Point3& leverArm);
// enable serialization functionality
void serialize() const;
@ -416,11 +490,49 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
const double& measurementIn() const;
double heightOut(double n) const;
double baroOut(const double& meters) const;
gtsam::Vector evaluateError(const gtsam::Pose3& p, double b);
// enable serialization functionality
void serialize() const;
};
#include <gtsam/navigation/ConstantVelocityFactor.h>
class ConstantVelocityFactor : gtsam::NonlinearFactor {
ConstantVelocityFactor(size_t i, size_t j, double dt, const gtsam::noiseModel::Base* model);
gtsam::Vector evaluateError(const gtsam::NavState &x1, const gtsam::NavState &x2) const;
};
#include <gtsam/navigation/MagFactor.h>
class MagFactor: gtsam::NonlinearFactor {
MagFactor(size_t key, const gtsam::Point3& measured, double scale,
const gtsam::Unit3& direction, const gtsam::Point3& bias,
const gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::Rot2& nRb);
};
class MagFactor1: gtsam::NonlinearFactor {
MagFactor1(size_t key, const gtsam::Point3& measured, double scale,
const gtsam::Unit3& direction, const gtsam::Point3& bias,
const gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::Rot3& nRb);
};
#include <gtsam/geometry/Pose2.h>
#include <gtsam/navigation/MagPoseFactor.h>
template <POSE = {gtsam::Pose2, gtsam::Pose3}>
virtual class MagPoseFactor : gtsam::NoiseModelFactor {
MagPoseFactor(size_t pose_key,
const POSE::Translation& measured, double scale,
const POSE::Translation& direction, const POSE::Translation& bias,
const gtsam::noiseModel::Base* noiseModel);
MagPoseFactor(size_t pose_key,
const POSE::Translation& measured, double scale,
const POSE::Translation& direction, const POSE::Translation& bias,
const gtsam::noiseModel::Base* noiseModel, const POSE& body_P_sensor);
Vector evaluateError(const POSE& nRb);
};
#include <gtsam/navigation/Scenario.h>
virtual class Scenario {
gtsam::Pose3 pose(double t) const;

202
gtsam/navigation/navigation.md
View File

@ -2,35 +2,207 @@
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
## Classes
### 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
### 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
### 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
### 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.
## Simulation Tools
### Magnetic Field Integration
- **[Scenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Base class for defining motion scenarios.
- **[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](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
This section describes the classes primarily involved in Attitude and Heading Reference Systems (AHRS), which rely on gyroscope measurements for orientation preintegration.
```mermaid
classDiagram
direction LR
class PreintegratedRotationParams {
+Matrix3 gyroscopeCovariance
+Vector3 omegaCoriolis
+Pose3 body_P_sensor
}
class PreintegratedRotation {
+double deltaTij_
+Rot3 deltaRij_
+Matrix3 delRdelBiasOmega_
+integrateGyroMeasurement()
+biascorrectedDeltaRij()
}
PreintegratedRotation ..> PreintegratedRotationParams : uses
class PreintegratedAhrsMeasurements {
+Matrix3 preintMeasCov_
}
PreintegratedAhrsMeasurements --|> PreintegratedRotation : inherits
class AHRSFactor {
+evaluateError(...) Vector3
}
AHRSFactor ..> PreintegratedAhrsMeasurements : uses
```
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](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`).
3. **AHRS Preintegrated Measurements (`PreintegratedAhrsMeasurements`)**:
* 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](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.
## IMU Factor and Preintegration
This section describes the classes involved in preintegrating full IMU measurements (accelerometer and gyroscope) for use in factors like `ImuFactor` and `CombinedImuFactor`.
```mermaid
classDiagram
direction TD
class PreintegrationBase {
<<Abstract>>
+imuBias::ConstantBias biasHat_
+double deltaTij_
+resetIntegration()*
+integrateMeasurement()*
+biasCorrectedDelta()*
+predict()
+computeError()
}
class ManifoldPreintegration {
+NavState deltaXij_
+update()
}
ManifoldPreintegration --|> PreintegrationBase : implements
class TangentPreintegration {
+Vector9 preintegrated_
+update()
}
TangentPreintegration --|> PreintegrationBase : implements
class PreintegratedImuMeasurements {
+Matrix9 preintMeasCov_
}
PreintegratedImuMeasurements --|> ManifoldPreintegration : inherits
PreintegratedImuMeasurements --|> TangentPreintegration : inherits
class PreintegratedCombinedMeasurements {
+Matrix preintMeasCov_ (15x15)
}
PreintegratedCombinedMeasurements --|> ManifoldPreintegration : inherits
PreintegratedCombinedMeasurements --|> TangentPreintegration : inherits
class ImuFactor {
Pose3, Vector3, Pose3, Vector3, ConstantBias
+evaluateError(...) Vector9
}
ImuFactor ..> PreintegratedImuMeasurements : uses
class ImuFactor2 {
NavState, NavState, ConstantBias
+evaluateError(...) Vector9
}
ImuFactor2 ..> PreintegratedImuMeasurements : uses
class CombinedImuFactor {
Pose3, Vector3, Pose3, Vector3, ConstantBias
+evaluateError(...) Vector (15)
}
CombinedImuFactor ..> PreintegratedCombinedMeasurements : uses
```
```mermaid
classDiagram
direction LR
class PreintegratedRotationParams {
+Matrix3 gyroscopeCovariance
+Vector3 omegaCoriolis
+Pose3 body_P_sensor
}
class PreintegrationParams {
+Matrix3 accelerometerCovariance
+Vector3 n_gravity
}
PreintegrationParams --|> PreintegratedRotationParams : inherits
class PreintegrationCombinedParams {
+Matrix3 biasAccCovariance
+Matrix3 biasOmegaCovariance
}
PreintegrationCombinedParams --|> PreintegrationParams : inherits
```
The key components are:
1. **Parameters (`...Params`)**:
* `PreintegratedRotationParams`: Base parameter class (gyro noise, Coriolis, sensor pose).
* `PreintegrationParams`: Adds accelerometer noise, gravity vector, integration noise.
* `PreintegrationCombinedParams`: Adds parameters for bias random walk covariance.
2. **Preintegration Interface (`PreintegrationBase`)**:
* An abstract base class defining the common interface for different IMU preintegration methods. It manages the bias estimate used during integration (`biasHat_`) and the time interval (`deltaTij_`).
* Defines pure virtual methods for integration, bias correction, and state access.
3. **Preintegration Implementations**:
* `ManifoldPreintegration`: Concrete implementation of `PreintegrationBase`. Integrates directly on the `NavState` manifold, storing the result as a `NavState`. Corresponds to Forster et al. RSS 2015.
* `TangentPreintegration`: Concrete implementation of `PreintegrationBase`. Integrates increments in the 9D tangent space of `NavState`, storing the result as a `Vector9`.
4. **Preintegrated Measurements Containers**:
* `PreintegratedImuMeasurements`: Stores the result of standard IMU preintegration along with its 9x9 covariance (`preintMeasCov_`).
* `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](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.
- If false, `ManifoldPreintegration` is used. Please use this setting to get the exact implementation from {cite:t}`https://doi.org/10.1109/TRO.2016.2597321`.
- If true, `TangentPreintegration` is used. This does the integration on the tangent space of the NavState manifold.
- Using the combined IMU factor is not recommended. Typically biases evolve slowly, and hence a separate, lower frequency Markov chain on the bias is more appropriate.
- For short-duration experiments it is even recommended to use a single constant bias. Bias estimation is notoriously hard to tune/debug, and also acts as a "sink" for any modeling errors. Hence, starting with a constant bias is a good idea to get the rest of the pipeline working.
These components together provide a comprehensive framework for fusing inertial data with other sensor measurements in navigation and robotics applications.