BarometricFactor

Overview¶

The BarometricFactor (contributed by Peter Milani 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).

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.

Mathematical Formulation¶

Measurement Model¶

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). This $h_{std}$ becomes the factor’s internal measurement nT_.
$h_{std} = \text{heightOut}(P_{meas})$
(1)
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:
$e = (p_z + b) - h_{std}$
(2)
where:
- $p_z$ is the Z-coordinate of the Pose3 translation.
- $b$ is the estimated altitude bias (in meters).
- $h_{std}$ is the altitude calculated from the pressure measurement.

Bias Modeling¶

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).

Key Functionality / API¶

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.
evaluateError(pose, bias): Calculates the 1D error $e = (pose.z() + bias) - h_{std}$ .
measurementIn(): Returns the internally stored altitude measurement $h_{std}$ (converted from the input pressure).
heightOut(pressure_kPa): Converts pressure (kPa) to altitude (m) using the standard atmosphere model.
baroOut(altitude_m): Converts altitude (m) back to pressure (kPa) using the inverse model.
print / equals: Standard factor methods.

Usage Example¶

Assume we have a pressure reading and want to add a factor constraining a Pose3 and a bias.

import gtsam
import numpy as np
from gtsam.symbol_shorthand import X, B # Pose key, Bias key

# Define keys
pose_key = X(0)
bias_key = B(0)

# Measurement
pressure_kPa = 100.1 # Example pressure reading in kilopascals

# Noise model on the *altitude* error (e.g., +/- 1 meter sigma)
altitude_sigma = 1.0 
noise_model = gtsam.noiseModel.Isotropic.Sigma(1, altitude_sigma)

# Create the factor
barometric_factor = gtsam.BarometricFactor(pose_key, bias_key, pressure_kPa, noise_model)

print("Created BarometricFactor:")
barometric_factor.print()

# Internal altitude measurement (converted from pressure)
internal_altitude_measurement = barometric_factor.measurementIn()
print(f"\nInternal altitude measurement: {internal_altitude_measurement:.2f} m")

# Example: Evaluate error 
# Assume current state estimate is Pose3 at z=110m and bias= -1.5m
current_pose = gtsam.Pose3(gtsam.Rot3(), np.array([0, 0, 110.0]))
current_bias = -1.5

error = barometric_factor.evaluateError(current_pose, current_bias)
predicted_altitude = current_pose.z() + current_bias
print(f"Current Pose Z: {current_pose.z():.2f} m")
print(f"Current Bias: {current_bias:.2f} m")
print(f"Predicted Altitude (Pose Z + Bias): {predicted_altitude:.2f} m")
print(f"Error (Predicted - Measured): {error[0]:.2f} m")

Created BarometricFactor:
Barometric Factor on x0Barometric Bias on b0
  Baro measurement: 108.939
  noise model: unit (1) 

Internal altitude measurement: 108.94 m
Current Pose Z: 110.00 m
Current Bias: -1.50 m
Predicted Altitude (Pose Z + Bias): 108.50 m
Error (Predicted - Measured): -0.44 m