223 lines
10 KiB
Plaintext
223 lines
10 KiB
Plaintext
{
|
|
"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
|
|
}
|