215 lines
7.7 KiB
Plaintext
215 lines
7.7 KiB
Plaintext
{
|
|
"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
|
|
}
|