astrodynamics-gnss 0.14.0

//! Exponential-scale-height ZWD troposphere delay variant.

use std::f64::consts::PI;

pub const TROPOSPHERE_ALTITUDE_CLAMP_M: (f64, f64) = (-500.0, 9000.0);

/// Closed altitude interval used before evaluating the ZWD profile.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct AltitudeClamp {
    pub min_m: f64,
    pub max_m: f64,
}

impl Default for AltitudeClamp {
    fn default() -> Self {
        Self {
            min_m: TROPOSPHERE_ALTITUDE_CLAMP_M.0,
            max_m: TROPOSPHERE_ALTITUDE_CLAMP_M.1,
        }
    }
}

/// Exponential wet-delay profile.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ZwdProfile {
    pub altitude_clamp: AltitudeClamp,
    pub sea_level_zenith_wet_delay_m: f64,
    pub wet_scale_height_m: f64,
}

impl Default for ZwdProfile {
    fn default() -> Self {
        Self {
            altitude_clamp: AltitudeClamp::default(),
            sea_level_zenith_wet_delay_m: 0.25,
            wet_scale_height_m: 2000.0,
        }
    }
}

/// Time input needed by the ZWD mapping variant.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct ZwdEpoch {
    pub unix_nanos: i64,
    pub day_of_year: u16,
}

impl ZwdEpoch {
    pub fn new(unix_nanos: i64, day_of_year: u16) -> Self {
        Self {
            unix_nanos,
            day_of_year,
        }
    }
}

/// Inputs controlling the XYZ ZWD slant-delay helper.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ZwdSlantOptions {
    pub epoch: ZwdEpoch,
    pub profile: ZwdProfile,
}

impl ZwdSlantOptions {
    pub const fn new(epoch: ZwdEpoch, profile: ZwdProfile) -> Self {
        Self { epoch, profile }
    }
}

#[derive(Clone, Copy, Debug)]
pub struct Mapping {
    pub m_hydrostatic: f64,
    pub m_wet: f64,
}

pub fn niell_mapping_function(
    elevation_rad: f64,
    latitude_deg: f64,
    day_of_year: u16,
    height_m: f64,
) -> Mapping {
    let mut sin_e = elevation_rad.sin();
    if sin_e < 0.01 {
        sin_e = 0.01;
    }

    let lat_rad = latitude_deg * PI / 180.0;
    let sin_lat = lat_rad.sin();
    let phi = 2.0 * PI * (day_of_year as f64 - 1.0) / 365.25;

    let a_h_base = 2.65e-3;
    let b_h_base = 2.3e-4;
    let c_h_base = 1.2e-4;

    let mut a_h = a_h_base * (1.0 - 0.0025 * sin_lat * sin_lat);
    let mut b_h = b_h_base * (1.0 - 0.0025 * sin_lat * sin_lat);
    let mut c_h = c_h_base * (1.0 - 0.0025 * sin_lat * sin_lat);

    a_h += 0.0005 * phi.cos() * (1.0 - 0.5 * sin_lat * sin_lat);
    b_h += 0.0001 * phi.cos() * (1.0 - 0.5 * sin_lat * sin_lat);
    c_h += 0.00005 * phi.cos() * (1.0 - 0.5 * sin_lat * sin_lat);

    let a_w_base = 1.5e-2;
    let b_w_base = 8.3e-3;
    let c_w_base = 1.0e-3;

    let mut a_w = a_w_base * (1.0 - 0.01 * sin_lat.abs());
    let mut b_w = b_w_base * (1.0 - 0.01 * sin_lat.abs());
    let mut c_w = c_w_base * (1.0 - 0.01 * sin_lat.abs());

    a_w += 0.005 * phi.cos() * (1.0 - sin_lat.abs());
    b_w += 0.003 * phi.cos() * (1.0 - sin_lat.abs());
    c_w += 0.0005 * phi.cos() * (1.0 - sin_lat.abs());

    if height_m > 0.0 {
        let height_factor = (-height_m / 8000.0).exp();
        a_h *= height_factor;
        b_h *= height_factor;
        c_h *= height_factor;
    }

    Mapping {
        m_hydrostatic: mapping_fraction(a_h, b_h, c_h, sin_e),
        m_wet: mapping_fraction(a_w, b_w, c_w, sin_e),
    }
}

pub fn tropo_delay_xyz<F>(
    options: ZwdSlantOptions,
    sat_xyz: &[f64; 3],
    receiver_xyz: &[f64; 3],
    ecef_to_lla: F,
) -> f64
where
    F: Fn(&[f64; 3]) -> [f64; 3],
{
    let receiver_sat_vector = [
        sat_xyz[0] - receiver_xyz[0],
        sat_xyz[1] - receiver_xyz[1],
        sat_xyz[2] - receiver_xyz[2],
    ];
    let receiver_up = unit_vector(receiver_xyz);
    let sat_unit = unit_vector(&receiver_sat_vector);
    let elevation_rad = dot_three_reference(&sat_unit, &receiver_up).asin();

    let lonlatalt = ecef_to_lla(receiver_xyz);
    let latitude = lonlatalt[1];
    let altitude = clamp(
        lonlatalt[2],
        options.profile.altitude_clamp.min_m,
        options.profile.altitude_clamp.max_m,
    );

    let zhd_m = saastamoinen_zhd(standard_pressure_hpa(altitude), latitude, altitude);
    let zwd_m = zwd_altitude_scaled(options.profile, altitude);
    let mapping =
        niell_mapping_function(elevation_rad, latitude, options.epoch.day_of_year, altitude);

    let slant_hyd = zhd_m * mapping.m_hydrostatic;
    let slant_wet = zwd_m * mapping.m_wet;
    slant_hyd + slant_wet
}

fn standard_pressure_hpa(altitude_m: f64) -> f64 {
    1013.25 * (1.0 - 2.25577e-5 * altitude_m).powf(5.2559)
}

fn saastamoinen_zhd(pressure_hpa: f64, latitude_deg: f64, altitude_m: f64) -> f64 {
    let lat_rad = latitude_deg * PI / 180.0;
    0.0022768 * pressure_hpa / (1.0 - 0.00266 * (2.0 * lat_rad).cos() - 2.8e-7 * altitude_m)
}

pub fn zenith_wet_delay(profile: ZwdProfile, height_m: f64) -> f64 {
    let altitude_m = clamp(
        height_m,
        profile.altitude_clamp.min_m,
        profile.altitude_clamp.max_m,
    );
    zwd_altitude_scaled(profile, altitude_m)
}

fn zwd_altitude_scaled(profile: ZwdProfile, altitude_m: f64) -> f64 {
    profile.sea_level_zenith_wet_delay_m * (-altitude_m / profile.wet_scale_height_m).exp()
}

fn mapping_fraction(a: f64, b: f64, c: f64, sin_e: f64) -> f64 {
    let numerator = 1.0 + a / (1.0 + b / (1.0 + c));
    let denominator = sin_e + a / (sin_e + b / (sin_e + c));
    numerator / denominator
}

fn unit_vector(vector: &[f64; 3]) -> [f64; 3] {
    let norm = dot_three(vector, vector).sqrt();
    [vector[0] / norm, vector[1] / norm, vector[2] / norm]
}

fn dot_three(a: &[f64; 3], b: &[f64; 3]) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}

fn dot_three_reference(a: &[f64; 3], b: &[f64; 3]) -> f64 {
    a[2] * b[2] + (a[1] * b[1] + a[0] * b[0])
}

fn clamp(v: f64, lo: f64, hi: f64) -> f64 {
    if v < lo {
        lo
    } else if v > hi {
        hi
    } else {
        v
    }
}