astrodynamics-gnss 0.9.4

//! GLONASS broadcast ephemeris: PZ-90.11 state-vector propagation (Phase 5b).
//!
//! GLONASS does not broadcast Keplerian elements. Each record carries an ECEF
//! state at a reference epoch — position, velocity, and the lunisolar
//! acceleration — plus the clock terms (−TauN, +GammaN). The position at another
//! epoch comes from numerically integrating the equation of motion (central
//! gravity + the J2 oblateness term + the Earth-rotation / Coriolis terms + the
//! broadcast lunisolar acceleration held constant) with a fixed-step fourth-order
//! Runge–Kutta integrator — the canonical GLONASS-ICD / RTKLIB `glorbit`/`deq`
//! algorithm.
//!
//! This reproduces the reference recipe `parity/generator/glonass_eval.py`
//! statement-for-statement: plain `f64` arithmetic with no fused multiply-add and
//! integer powers written as explicit multiplies, so it is a bit-exact (0-ULP)
//! target against the committed `glonass_golden.json`. Unlike the closed-form
//! Keplerian path it is a numerical integrator, so it is additionally validated
//! physically against precise GLONASS orbits by the parity SP3 gate.
//!
//! The step is pinned at 60 s with a final partial step to land exactly on the
//! requested epoch, and the integration direction follows the sign of the time
//! difference. A different step policy is a different (still valid) answer, so it
//! is part of the pinned contract.

/// Geocentric gravitational constant (m^3/s^2), PZ-90.11.
pub const MU: f64 = 3.9860044e14;
/// Second zonal harmonic of the geopotential, PZ-90.11.
pub const J2: f64 = 1.0826257e-3;
/// Earth rotation rate (rad/s), PZ-90.11.
pub const OMEGA_E: f64 = 7.292115e-5;
/// Earth equatorial radius (m), PZ-90.11.
pub const R_E: f64 = 6378136.0;
/// Pinned RK4 fixed step (seconds).
pub const TSTEP_S: f64 = 60.0;
/// Loop-termination tolerance (seconds) for the residual time.
const TOL_S: f64 = 1.0e-9;

/// State derivative for the GLONASS equation of motion.
///
/// `s` is `[x, y, z, vx, vy, vz]` in metres and metres/second (PZ-90 ECEF); `acc`
/// is the broadcast lunisolar acceleration `[ax, ay, az]` in m/s^2, held constant
/// over the step. Returns `[vx, vy, vz, ax, ay, az]`.
pub(crate) fn deq(s: &[f64; 6], acc: &[f64; 3]) -> [f64; 6] {
    let (x, y, z, vx, vy, vz) = (s[0], s[1], s[2], s[3], s[4], s[5]);
    let r2 = x * x + y * y + z * z;
    let r = r2.sqrt();
    let r3 = r2 * r;
    // a = 3/2 * J2 * mu * Re^2 / r^5, formed as (3/2 J2 mu Re^2) / (r^2 * r^3).
    let a = 1.5 * J2 * MU * (R_E * R_E) / (r2 * r3);
    let b = 5.0 * z * z / r2;
    let c = -MU / r3 - a * (1.0 - b);
    let omg2 = OMEGA_E * OMEGA_E;
    let ax = (c + omg2) * x + 2.0 * OMEGA_E * vy + acc[0];
    let ay = (c + omg2) * y - 2.0 * OMEGA_E * vx + acc[1];
    let az = (c - 2.0 * a) * z + acc[2];
    [vx, vy, vz, ax, ay, az]
}

/// One classical RK4 step of length `step` seconds.
#[allow(clippy::needless_range_loop)] // explicit indices keep the RK4 stage updates legible and pinned
pub(crate) fn glorbit(step: f64, s: &[f64; 6], acc: &[f64; 3]) -> [f64; 6] {
    let k1 = deq(s, acc);
    let mut w = [0.0_f64; 6];
    for i in 0..6 {
        w[i] = s[i] + k1[i] * step / 2.0;
    }
    let k2 = deq(&w, acc);
    for i in 0..6 {
        w[i] = s[i] + k2[i] * step / 2.0;
    }
    let k3 = deq(&w, acc);
    for i in 0..6 {
        w[i] = s[i] + k3[i] * step;
    }
    let k4 = deq(&w, acc);
    let mut out = [0.0_f64; 6];
    for i in 0..6 {
        out[i] = s[i] + (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * step / 6.0;
    }
    out
}

/// Integrate `state0` by `tk` seconds with the pinned step policy: fixed 60 s
/// steps in the direction of `tk`, then a final partial step of the remainder.
pub fn propagate(state0: [f64; 6], acc: [f64; 3], tk: f64) -> [f64; 6] {
    let mut state = state0;
    let mut tt = tk;
    while tt.abs() > TOL_S {
        let step = if tt.abs() < TSTEP_S {
            tt
        } else if tt > 0.0 {
            TSTEP_S
        } else {
            -TSTEP_S
        };
        state = glorbit(step, &state, &acc);
        tt -= step;
    }
    state
}

/// GLONASS clock offset (seconds) at `tk` = t − toe.
///
/// `clk_bias` is the broadcast line-0 field (which is −TauN) and `gamma_n` is
/// +GammaN. The time argument is refined twice for the small clock-vs-signal time
/// difference. There is no relativistic eccentricity term and no group delay for
/// the basic single-frequency user.
pub fn clock_offset_s(clk_bias: f64, gamma_n: f64, tk: f64) -> f64 {
    let mut t = tk;
    for _ in 0..2 {
        t -= clk_bias + gamma_n * t;
    }
    clk_bias + gamma_n * t
}

#[cfg(test)]
mod tests;