225 lines
9.8 KiB
Plaintext
225 lines
9.8 KiB
Plaintext
{
|
|
"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
|
|
}
|