diff --git a/gtsam/navigation/doc/BarometricFactor.ipynb b/gtsam/navigation/doc/BarometricFactor.ipynb new file mode 100644 index 000000000..f15bbaaba --- /dev/null +++ b/gtsam/navigation/doc/BarometricFactor.ipynb @@ -0,0 +1,169 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# BarometricFactor\n", + "\n", + "\"Open\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` 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 +} diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index ba186b3a0..30bec16b9 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -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;