Added barometric altitude calculations

Author: jbrodovsky

core/src/earth.rs

@@ -32,21 +32,9 @@
 //! are primarily used to convert between the ECEF and local-level frames. The rotations
 //! from the local level frame to the body frame can be taken care of by the `nalgebra`
 //! crate, which provides the necessary rotation matrices using the Rotation3 type.
-
-// ----------
-// Working notes:
-// The canonical strapdown navigation state vector is WGS84 geodetic position (latitude, longitude, altitude) and local tangent plane (NED) velocities (north, east,
-// down). The state vector is updated by integrating the IMU measurements (body frame) to estimate the position and velocity of the sensor. Velocities are updated
-// in NED, whereas the positions are updated in WGS84.
-// ----------
-// Rotations can be handled using nalgebra's Rotation3 type, which can be converted to a DCM using the into() method. The Rotation3 type can be created from
-// Euler angles for the body to local-level frame rotation. The inverse of the Rotation3 type can be used to convert from the local-level frame to the body frame.
-// ----------
 use crate::{wrap_latitude, wrap_to_180};
 use ::nalgebra::{Matrix3, Vector3};
 use ::nav_types::{ECEF, WGS84};
-
-// Earth constants (WGS84)
 /// Earth's rotation rate rad/s ($\omega_{ie}$)
 pub const RATE: f64 = 7.2921159e-5;
 /// Earth's rotation rate rad/s ($\omega_{ie}$) in a vector form
@@ -65,6 +53,8 @@ pub const ECCENTRICITY_SQUARED: f64 = ECCENTRICITY * ECCENTRICITY;
 pub const GE: f64 = 9.7803253359; // m/s^2, equatorial radius
 /// Earth's gravitational acceleration at the poles ($g_p$) in $m/s^2$
 pub const GP: f64 = 9.8321849378; // $m/s^2$, polar radius
+/// Earth's average gravitational acceleration ($g$) in $m/s^2$
+pub const G0: f64 = 9.80665; // m/s^2, average gravitational acceleration
 /// Earth's flattening factor ($f$)
 pub const F: f64 = 1.0 / 298.257223563; // Flattening factor
 /// Somigliana's constant ($K$)
@@ -82,7 +72,49 @@ pub const MAGNETIC_FIELD_STRENGTH: f64 = 3.12e-5; // T, reference mean magnetic
 pub const METERS_TO_DEGREES: f64 = 1.0 / (60.0 * 1852.0);
 /// Rough conversion factor from degrees to meters for latitude/longitude via nautical miles (1 degree ~ 60 nautical miles; 1 nautical mile ~ 1852 meters)
 pub const DEGREES_TO_METERS: f64 = 60.0 * 1852.0;
-
+/// Sea-level atmospheric pressure in pascals 
+pub const SEA_LEVEL_PRESSURE: f64 = 101325.0; // Pa, standard sea-level pressure
+/// Temperature lapse rate in Kelvin per meter
+pub const TEMPERATURE_LAPSE_RATE: f64 = 0.00976; // K/m, standard temperature lapse rate in the troposphere
+/// Constant-pressure specific heat
+pub const CONSTANT_PRESSURE_SPECIFIC_HEAT: f64 = 1004.68506; // J/(kg·K), specific heat at constant pressure for dry air
+/// Sea level standard temperature in Kelvin
+pub const SEA_LEVEL_STANDARD_TEMPERATURE: f64 = 288.15; // K, standard sea-level temperature
+/// Molar mass of dry air in kg/mol
+pub const MOLAR_MASS_DRY_AIR: f64 = 0.02896968; // kg/mol, molar mass of dry air
+/// Universal gas constant in J/(mol·K)
+pub const UNIVERSAL_GAS_CONSTANT: f64 = 8.314462618; // J/(mol·K), universal gas constant
+/// Calculate the barometric altitude from pressure and temperature
+/// 
+/// This function calculates the barometric altitude using the barometric formula, which relates
+/// the pressure at a given altitude to the pressure at sea level. The formula is based on the
+/// assumption of an isothermal atmosphere and the ideal gas law.
+/// 
+/// # Arguments
+/// - `pressure` - The atmospheric pressure at the given altitude in pascals
+/// 
+/// # Returns
+/// The barometric altitude in meters
+pub fn barometric_altitude(pressure: f64) -> f64 {
+    let exponent: f64 = -((UNIVERSAL_GAS_CONSTANT * TEMPERATURE_LAPSE_RATE) / (G0 * MOLAR_MASS_DRY_AIR));
+    (SEA_LEVEL_STANDARD_TEMPERATURE / TEMPERATURE_LAPSE_RATE) * (pressure / SEA_LEVEL_PRESSURE) * exponent.exp()
+}
+/// Calculates the relative barometric altitude from pressure and temperature
+/// 
+/// This function calculates the relative barometric altitude using the barometric formula,
+/// which relates the pressure at a given altitude to the pressure at a reference altitude.
+/// This is similar to how aircraft pressure altimeters work, where the preassure at a known
+/// altitude is used to calibrate the altimeter.
+/// 
+/// # Arguments
+/// - `pressure` - The measured atmospheric pressure
+/// - `initial_pressure` - The atmospheric pressure at the reference altitude in pascals
+/// 
+/// # Returns
+/// The relative barometric altitude in meters, i.e. the altitude relative to the reference.
+pub fn relative_barometric_altitude(pressure: f64, initial_pressure: f64) -> f64 {
+    ((UNIVERSAL_GAS_CONSTANT * SEA_LEVEL_PRESSURE) / (G0 * MOLAR_MASS_DRY_AIR)) * (initial_pressure / pressure).ln()
+}
 /// Convert a three-element vector to a skew-symmetric matrix
 ///
 /// Groves' notation uses a lot of skew-symmetric matrices to represent cross products
@@ -141,7 +173,7 @@ pub fn skew_symmetric_to_vector(skew: &Matrix3<f64>) -> Vector3<f64> {
 /// system with the origin at the Earth's center. The ECI frame is fixed with respect to the
 /// stars, whereas the ECEF frame rotates with the Earth.
 ///
-/// # Parameters
+/// # Arguments
 /// - `time` - The time in seconds that define the rotation period
 ///
 /// # Returns
@@ -171,7 +203,7 @@ pub fn eci_to_ecef(time: f64) -> Matrix3<f64> {
 /// system with the origin at the Earth's center. The ECI frame is fixed with respect to the
 /// stars, whereas the ECEF frame rotates with the Earth.
 ///     
-/// # Parameters
+/// # Arguments
 /// - `time` - The time in seconds that define the rotation period
 ///
 /// # Returns
@@ -194,7 +226,7 @@ pub fn ecef_to_eci(time: f64) -> Matrix3<f64> {
 /// position. The local-level frame is defined by the tangent to the ellipsoidal surface at the sensor's
 /// position. The local level frame is defined by the WGS84 latitude and longitude.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `longitude` - The WGS84 longitude in degrees
 ///
@@ -231,7 +263,7 @@ pub fn ecef_to_lla(latitude: &f64, longitude: &f64) -> Matrix3<f64> {
 /// position. The local-level frame is defined by the tangent to the ellipsoidal surface at the sensor's
 /// position. The local level frame is defined by the WGS84 latitude and longitude.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `longitude` - The WGS84 longitude in degrees
 ///
@@ -254,7 +286,7 @@ pub fn lla_to_ecef(latitude: &f64, longitude: &f64) -> Matrix3<f64> {
 /// The [principal radii of curvature](https://en.wikipedia.org/wiki/Earth_radius) are used to
 /// calculate and convert Cartesian body frame quantities to the local-level frame WGS84 coordinates.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `altitude` - The WGS84 altitude in meters
 ///
@@ -288,7 +320,7 @@ pub fn principal_radii(latitude: &f64, altitude: &f64) -> (f64, f64, f64) {
 /// the Earth's gravity as a function of the latitude and altitude. The gravity model is used to
 /// calculate the gravitational force scalar in the local-level frame. Free-air correction is applied.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `altitude` - The WGS84 altitude in meters
 ///
@@ -322,7 +354,7 @@ pub fn gravity(latitude: &f64, altitude: &f64) -> f64 {
 /// Down (NED) or East, North, Up (ENU). This function uses the ENU convention, thus gravity acts
 /// along the negative Z-axis (downward) in the local-level frame.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `longitude` - The WGS84 longitude in degrees
 /// - `altitude` - The WGS84 altitude in meters
@@ -359,7 +391,7 @@ pub fn gravitation(latitude: &f64, longitude: &f64, altitude: &f64) -> Vector3<f
 /// at the given latitude and altitude via the Somigliana method. Additionally, this function
 /// compensates for the motion of the platform (if any) using the Eötvös correction.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `altitude` - The WGS84 altitude in meters
 /// - `north_velocity` - The northward velocity component in m/s
@@ -406,7 +438,7 @@ pub fn eotvos(latitude: &f64, altitude: &f64, north_velocity: &f64, east_velocit
 /// The Earth's rotation rate modeled as a vector in the local-level frame. This vector
 /// is used to calculate the Coriolis and centrifugal effects in the local-level frame.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 ///
 /// # Returns
@@ -430,7 +462,7 @@ pub fn earth_rate_lla(latitude: &f64) -> Vector3<f64> {
 /// with respect to the ECEF frame since the origin point of the local-level frame is
 /// always tangential to the WGS84 ellipsoid and thus constantly moving in the ECEF frame.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `altitude` - The WGS84 altitude in meters
 /// - `velocities` - The velocity vector in the local-level frame (northward, eastward, downward)
@@ -463,7 +495,7 @@ pub fn transport_rate(latitude: &f64, altitude: &f64, velocities: &Vector3<f64>)
 /// dipole model. The dipole model approximates the Earth's magnetic field as a magnetic
 /// dipole with the axis through the geographic poles.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `longitude` - The WGS84 longitude in degrees  
 /// - `altitude` - The WGS84 altitude in meters
@@ -496,7 +528,7 @@ pub fn calculate_magnetic_field(latitude: &f64, longitude: &f64, altitude: &f64)
 
 /// Calculate the radial component of Earth's magnetic field using the dipole model
 ///
-/// # Parameters
+/// # Arguments
 /// - `colatitude` - The magnetic *colatitude* in radians (angle from magnetic north pole)
 /// - `radius` - The distance from Earth's center in meters
 ///
@@ -518,7 +550,7 @@ pub fn calculate_radial_magnetic_field(colatitude: f64, radius: f64) -> f64 {
 
 /// Calculate the latitudinal component of Earth's magnetic field using the dipole model
 ///
-/// # Parameters
+/// # Arguments
 /// - `colatitude` - The magnetic *colatitude* in radians (angle from magnetic north pole)
 /// - `radius` - The distance from Earth's center in meters
 ///
@@ -541,7 +573,7 @@ pub fn calculate_latitudinal_magnetic_field(colatitude: f64, radius: f64) -> f64
 /// using the dipole model of Earth's magnetic field. The transformation is based on
 /// the location of the geomagnetic north pole.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `longitude` - The WGS84 longitude in degrees
 ///
@@ -587,7 +619,7 @@ pub fn wgs84_to_magnetic(latitude: &f64, longitude: &f64) -> (f64, f64) {
 /// magnetic field vector, positive downward. At the magnetic equator, the inclination
 /// is zero. At the magnetic poles, the inclination is ±90°.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `longitude` - The WGS84 longitude in degrees
 /// - `altitude` - The WGS84 altitude in meters
@@ -621,7 +653,7 @@ pub fn magnetic_inclination(latitude: &f64, longitude: &f64, altitude: &f64) ->
 /// positive eastward. This is essential for navigation as it represents the correction
 /// needed to convert between magnetic compass readings and true bearings.
 ///
-/// # Parameters
+/// # Arguments
 /// - `latitude` - The WGS84 latitude in degrees
 /// - `longitude` - The WGS84 longitude in degrees
 /// - `altitude` - The WGS84 altitude in meters