Almost perfect Gemini 2.5 notebooks
Frank Dellaert
parent
f991a42279
commit
e84b1c4ec0
|
|
@ -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"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -0,0 +1,222 @@
|
|||
{
|
||||
"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",
|
||||
"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",
|
||||
"**Important Note:** This factor assumes the bias is *constant* between time $i$ and $j$ for the purpose of evaluating its error. It does *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead."
|
||||
]
|
||||
},
|
||||
{
|
||||
"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": 3,
|
||||
"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": [
|
||||
"## 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
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
Loading…
Reference in New Issue