BarometricFactor
Frank Dellaert
parent
13a74a4af0
commit
8757090b7a
|
|
@ -0,0 +1,169 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# BarometricFactor\n",
|
||||
"\n",
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/BarometricFactor.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 `BarometricFactor` (contributed by [Peter Milani](https://github.com/pmmilani) in 2021) provides a way to incorporate altitude information derived from barometric pressure measurements into a GTSAM factor graph. It acts as a unary factor on a `Pose3` state variable but also connects to a `double` variable representing a slowly varying atmospheric pressure bias (or equivalently, an altitude offset).\n",
|
||||
"\n",
|
||||
"Barometers measure absolute atmospheric pressure. Under the assumption of a standard atmosphere model, this pressure can be converted into an estimate of altitude above sea level. However, local atmospheric pressure changes constantly due to weather, making the direct pressure-to-altitude conversion inaccurate over longer periods. The `BarometricFactor` accounts for this by simultaneously estimating the vehicle's altitude (Z-component of the `Pose3`) and a bias term that absorbs the slow variations in local atmospheric pressure relative to the standard model."
|
||||
]
|
||||
},
|
||||
{
|
||||
"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",
|
||||
"### Measurement Model\n",
|
||||
"\n",
|
||||
"1. **Pressure to Altitude:** The factor internally converts the input barometric pressure measurement $P_{meas}$ (in kPa) to an altitude estimate $h_{std}$ (in meters) using a standard atmosphere model (approximated by the factor's `heightOut` method, based on [NASA GRC](https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html)). This $h_{std}$ becomes the factor's internal measurement `nT_`.\n",
|
||||
" $$ h_{std} = \\text{heightOut}(P_{meas}) $$ \n",
|
||||
"\n",
|
||||
"2. **Factor Error:** The factor constrains the Z-component of the `Pose3` variable's translation ($p_z$) and the bias variable ($b$). The error is the difference between the altitude predicted by the state and bias, and the altitude derived from the measurement:\n",
|
||||
" $$ e = (p_z + b) - h_{std} $$ \n",
|
||||
" where:\n",
|
||||
" - $p_z$ is the Z-coordinate of the `Pose3` translation.\n",
|
||||
" - $b$ is the estimated altitude bias (in meters).\n",
|
||||
" - $h_{std}$ is the altitude calculated from the pressure measurement.\n",
|
||||
"\n",
|
||||
"### Bias Modeling\n",
|
||||
"\n",
|
||||
"The bias $b$ is typically connected between successive time steps using a `BetweenFactor<double>` with a very small noise model, enforcing that the bias changes slowly over time (approximating a random walk)."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Key Functionality / API\n",
|
||||
"\n",
|
||||
"- **Constructor**: `BarometricFactor(poseKey, biasKey, baroIn_kPa, model)`: Creates the factor, taking the `Pose3` key, the `double` bias key, the pressure measurement *in kilopascals (kPa)*, and the 1D noise model for the altitude error.\n",
|
||||
"- **`evaluateError(pose, bias)`**: Calculates the 1D error $e = (pose.z() + bias) - h_{std}$.\n",
|
||||
"- **`measurementIn()`**: Returns the internally stored altitude measurement $h_{std}$ (converted from the input pressure).\n",
|
||||
"- **`heightOut(pressure_kPa)`**: Converts pressure (kPa) to altitude (m) using the standard atmosphere model.\n",
|
||||
"- **`baroOut(altitude_m)`**: Converts altitude (m) back to pressure (kPa) using the inverse model.\n",
|
||||
"- **`print` / `equals`**: Standard factor methods."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Usage Example\n",
|
||||
"\n",
|
||||
"Assume we have a pressure reading and want to add a factor constraining a `Pose3` and a bias."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Created BarometricFactor:\n",
|
||||
"Barometric Factor on x0Barometric Bias on b0\n",
|
||||
" Baro measurement: 108.939\n",
|
||||
" noise model: unit (1) \n",
|
||||
"\n",
|
||||
"Internal altitude measurement: 108.94 m\n",
|
||||
"Current Pose Z: 110.00 m\n",
|
||||
"Current Bias: -1.50 m\n",
|
||||
"Predicted Altitude (Pose Z + Bias): 108.50 m\n",
|
||||
"Error (Predicted - Measured): -0.44 m\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import gtsam\n",
|
||||
"import numpy as np\n",
|
||||
"from gtsam.symbol_shorthand import X, B # Pose key, Bias key\n",
|
||||
"\n",
|
||||
"# Define keys\n",
|
||||
"pose_key = X(0)\n",
|
||||
"bias_key = B(0)\n",
|
||||
"\n",
|
||||
"# Measurement\n",
|
||||
"pressure_kPa = 100.1 # Example pressure reading in kilopascals\n",
|
||||
"\n",
|
||||
"# Noise model on the *altitude* error (e.g., +/- 1 meter sigma)\n",
|
||||
"altitude_sigma = 1.0 \n",
|
||||
"noise_model = gtsam.noiseModel.Isotropic.Sigma(1, altitude_sigma)\n",
|
||||
"\n",
|
||||
"# Create the factor\n",
|
||||
"barometric_factor = gtsam.BarometricFactor(pose_key, bias_key, pressure_kPa, noise_model)\n",
|
||||
"\n",
|
||||
"print(\"Created BarometricFactor:\")\n",
|
||||
"barometric_factor.print()\n",
|
||||
"\n",
|
||||
"# Internal altitude measurement (converted from pressure)\n",
|
||||
"internal_altitude_measurement = barometric_factor.measurementIn()\n",
|
||||
"print(f\"\\nInternal altitude measurement: {internal_altitude_measurement:.2f} m\")\n",
|
||||
"\n",
|
||||
"# Example: Evaluate error \n",
|
||||
"# Assume current state estimate is Pose3 at z=110m and bias= -1.5m\n",
|
||||
"current_pose = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 110.0]))\n",
|
||||
"current_bias = -1.5\n",
|
||||
"\n",
|
||||
"error = barometric_factor.evaluateError(current_pose, current_bias)\n",
|
||||
"predicted_altitude = current_pose.z() + current_bias\n",
|
||||
"print(f\"Current Pose Z: {current_pose.z():.2f} m\")\n",
|
||||
"print(f\"Current Bias: {current_bias:.2f} m\")\n",
|
||||
"print(f\"Predicted Altitude (Pose Z + Bias): {predicted_altitude:.2f} m\")\n",
|
||||
"print(f\"Error (Predicted - Measured): {error[0]:.2f} m\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Source\n",
|
||||
"- [BarometricFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.h)\n",
|
||||
"- [BarometricFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.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
|
||||
}
|
||||
|
|
@ -382,6 +382,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
|
|||
|
||||
// Standard Interface
|
||||
gtsam::Point3 measurementIn() const;
|
||||
gtsam::Vector evaluateError(const gtsam::Pose3& nTb);
|
||||
|
||||
// enable serialization functionality
|
||||
void serialize() const;
|
||||
|
|
@ -398,6 +399,7 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
|
|||
|
||||
// Standard Interface
|
||||
gtsam::Point3 measurementIn() const;
|
||||
gtsam::Vector evaluateError(const gtsam::NavState& nTb);
|
||||
|
||||
// enable serialization functionality
|
||||
void serialize() const;
|
||||
|
|
@ -418,6 +420,7 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
|
|||
const double& measurementIn() const;
|
||||
double heightOut(double n) const;
|
||||
double baroOut(const double& meters) const;
|
||||
gtsam::Vector evaluateError(const gtsam::Pose3& p, double b);
|
||||
|
||||
// enable serialization functionality
|
||||
void serialize() const;
|
||||
|
|
|
|||
Loading…
Reference in New Issue