Scenario and ScenarioRunner
Frank Dellaert
parent
f4485db21c
commit
a68d3ffbd2
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
Loading…
Reference in New Issue