From 6eaaa86e30dad43935b64cfadf0a00a8fb6a9c12 Mon Sep 17 00:00:00 2001 From: ApsisRS contributors Date: Sat, 26 Sep 2026 14:16:23 +0800 Subject: [PATCH] orbit: add SGP4 and SDP4 propagator --- crates/orbit/apsis-sgp4/Cargo.toml | 4 + crates/orbit/apsis-sgp4/src/deep_space.rs | 1113 ++++++++++++++++++ crates/orbit/apsis-sgp4/src/error.rs | 70 ++ crates/orbit/apsis-sgp4/src/init.rs | 180 +++ crates/orbit/apsis-sgp4/src/lib.rs | 71 +- crates/orbit/apsis-sgp4/src/model.rs | 108 ++ crates/orbit/apsis-sgp4/src/propagate.rs | 299 +++++ crates/orbit/apsis-sgp4/src/satellite.rs | 416 +++++++ crates/orbit/apsis-sgp4/src/state.rs | 37 + crates/orbit/apsis-sgp4/tests/model.rs | 51 + crates/orbit/apsis-sgp4/tests/propagation.rs | 317 +++++ 11 files changed, 2665 insertions(+), 1 deletion(-) create mode 100644 crates/orbit/apsis-sgp4/src/deep_space.rs create mode 100644 crates/orbit/apsis-sgp4/src/error.rs create mode 100644 crates/orbit/apsis-sgp4/src/init.rs create mode 100644 crates/orbit/apsis-sgp4/src/model.rs create mode 100644 crates/orbit/apsis-sgp4/src/propagate.rs create mode 100644 crates/orbit/apsis-sgp4/src/satellite.rs create mode 100644 crates/orbit/apsis-sgp4/src/state.rs create mode 100644 crates/orbit/apsis-sgp4/tests/model.rs create mode 100644 crates/orbit/apsis-sgp4/tests/propagation.rs diff --git a/crates/orbit/apsis-sgp4/Cargo.toml b/crates/orbit/apsis-sgp4/Cargo.toml index a10d817..3bcfa50 100644 --- a/crates/orbit/apsis-sgp4/Cargo.toml +++ b/crates/orbit/apsis-sgp4/Cargo.toml @@ -11,3 +11,7 @@ publish = false workspace = true [dependencies] +apsis-core.workspace = true +apsis-math.workspace = true +apsis-tle.workspace = true +apsis-time.workspace = true diff --git a/crates/orbit/apsis-sgp4/src/deep_space.rs b/crates/orbit/apsis-sgp4/src/deep_space.rs new file mode 100644 index 0000000..c05f33b --- /dev/null +++ b/crates/orbit/apsis-sgp4/src/deep_space.rs @@ -0,0 +1,1113 @@ +//! The deep space branch of the propagator. +//! +//! Satellites with an orbital period of 225 minutes or more are propagated +//! with the SDP4 theory, which extends SGP4 with the luni-solar +//! perturbations and the resonance with the Earth's gravity field. +//! +//! The perturbations are split into three groups. Secular terms grow +//! linearly with time and are accumulated in the rate fields. Long period +//! terms vary with the position of the Sun and Moon and are applied by +//! [`dpper`]. Resonance terms arise when the orbital period is commensurate +//! with the Earth's rotation, either one revolution per day or two, and are +//! integrated numerically by [`dspace`] because no closed form exists. +//! +//! The coefficients are computed once at initialisation, since they depend +//! only on the epoch elements and the epoch time. +//! +//! Source: every expression follows Vallado et al., *Revisiting Spacetrack +//! Report #3*, AIAA 2006-6753, 2006, and the reference implementation +//! distributed with that paper. The perturbation amplitudes such as `zes` +//! and `znl`, the tesseral coefficients such as `q22`, and the resonance +//! coefficients are quoted to the precision the reference uses, because +//! reproducing its output requires the same rounding. + +use crate::satellite::{Satellite, TWO_PI}; +use core::f64::consts::PI; + +/// Solar eccentricity. +const ZES: f64 = 0.016_75; +/// Lunar eccentricity. +const ZEL: f64 = 0.054_90; +/// Solar mean motion coefficient, in radians per minute. +const ZNS: f64 = 1.194_59e-5; +/// Lunar mean motion coefficient, in radians per minute. +const ZNL: f64 = 1.583_521_8e-4; + +/// The rate of rotation of the Earth used by the resonance integral, in +/// radians per minute. +/// +/// This is `7.292 115 146 688 55e-5` radians per second. +const RPTIM: f64 = 4.375_269_088_011_3e-3; + +/// The integration step of the resonance, in minutes. +const STEP_POSITIVE: f64 = 720.0; +/// The integration step of the resonance for backward propagation. +const STEP_NEGATIVE: f64 = -720.0; +/// The square of the integration step, in minutes squared. +const STEP_SQUARED: f64 = 259_200.0; + +/// The values returned by the first stage of the deep space +/// initialisation, which computes the luni-solar series. +/// +/// The reference implementation names the two evaluations of the series with +/// a leading `s`: the values from the first pass, for the Moon, are stored +/// with a doubled letter, and the values from the second pass, for the Sun, +/// keep the single letter. The final coefficients combine one from each +/// group, so both are carried here. +#[derive(Debug, Clone, Copy)] +struct Lunisolar { + /// Moon series, second order. + ss1: f64, + /// Moon series, second order. + ss2: f64, + /// Moon series, second order. + ss3: f64, + /// Moon series, second order. + ss4: f64, + /// Moon series, second order. + ss5: f64, + /// Moon series, second order. + ss6: f64, + /// Moon series, second order. + ss7: f64, + /// Moon series, radial terms. + sz1: f64, + /// Moon series, radial terms. + sz2: f64, + /// Moon series, radial terms. + sz3: f64, + /// Moon series, radial terms. + sz11: f64, + /// Moon series, radial terms. + sz12: f64, + /// Moon series, radial terms. + sz13: f64, + /// Moon series, radial terms. + sz21: f64, + /// Moon series, radial terms. + sz22: f64, + /// Moon series, radial terms. + sz23: f64, + /// Moon series, radial terms. + sz31: f64, + /// Moon series, radial terms. + sz32: f64, + /// Moon series, radial terms. + sz33: f64, + /// Sun series, second order. + s1: f64, + /// Sun series, second order. + s2: f64, + /// Sun series, second order. + s3: f64, + /// Sun series, second order. + s4: f64, + /// Sun series, second order. + s5: f64, + /// Sun series, second order. + s6: f64, + /// Sun series, second order. + s7: f64, + /// Sun series, radial terms. + z1: f64, + /// Sun series, radial terms. + z2: f64, + /// Sun series, radial terms. + z3: f64, + /// Sun series, radial terms. + z11: f64, + /// Sun series, radial terms. + z12: f64, + /// Sun series, radial terms. + z13: f64, + /// Sun series, radial terms. + z21: f64, + /// Sun series, radial terms. + z22: f64, + /// Sun series, radial terms. + z23: f64, + /// Sun series, radial terms. + z31: f64, + /// Sun series, radial terms. + z32: f64, + /// Sun series, radial terms. + z33: f64, + /// Mean anomaly of the Moon at the epoch, in radians. + zmol: f64, + /// Mean anomaly of the Sun at the epoch, in radians. + zmos: f64, + /// Eccentricity squared at the epoch. + emsq: f64, + /// Sine of the inclination at the epoch. + sinim: f64, + /// Cosine of the inclination at the epoch. + cosim: f64, +} + +/// The deep space coefficients of a satellite. +/// +/// Every field is zero for a near-Earth orbit, so the structure can be +/// stored unconditionally and the near-Earth propagation step pays only the +/// cost of carrying the storage. +#[derive(Debug, Clone, Copy, PartialEq)] +pub(crate) struct DeepSpace { + /// Solar and lunar long period periodic coefficients. + pub(crate) e3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) ee2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) se2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) se3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) si2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) si3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sl2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sl3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sl4: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sgh2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sgh3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sgh4: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sh2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) sh3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xgh2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xgh3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xgh4: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xh2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xh3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xi2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xi3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xl2: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xl3: f64, + /// Solar and lunar long period periodic coefficients. + pub(crate) xl4: f64, + /// Mean anomaly of the Moon at the epoch, in radians. + pub(crate) zmol: f64, + /// Mean anomaly of the Sun at the epoch, in radians. + pub(crate) zmos: f64, + /// The long period periodic terms carried forward from the epoch. + pub(crate) peo: f64, + /// The long period periodic terms carried forward from the epoch. + pub(crate) pinco: f64, + /// The long period periodic terms carried forward from the epoch. + pub(crate) plo: f64, + /// The long period periodic terms carried forward from the epoch. + pub(crate) pgho: f64, + /// The long period periodic terms carried forward from the epoch. + pub(crate) pho: f64, + + /// Resonance flag: zero for none, one for the synchronous resonance, two + /// for the half-day resonance. + pub(crate) irez: i32, + /// Secular rate of the eccentricity, per minute. + pub(crate) dedt: f64, + /// Secular rate of the inclination, per minute. + pub(crate) didt: f64, + /// Secular rate of the mean anomaly, per minute. + pub(crate) dmdt: f64, + /// Secular rate of the right ascension, per minute. + pub(crate) dnodt: f64, + /// Secular rate of the argument of perigee, per minute. + pub(crate) domdt: f64, + /// Synchronous resonance coefficient. + pub(crate) del1: f64, + /// Synchronous resonance coefficient. + pub(crate) del2: f64, + /// Synchronous resonance coefficient. + pub(crate) del3: f64, + /// Half-day resonance coefficients. + pub(crate) d2201: f64, + /// Half-day resonance coefficients. + pub(crate) d2211: f64, + /// Half-day resonance coefficients. + pub(crate) d3210: f64, + /// Half-day resonance coefficients. + pub(crate) d3222: f64, + /// Half-day resonance coefficients. + pub(crate) d4410: f64, + /// Half-day resonance coefficients. + pub(crate) d4422: f64, + /// Half-day resonance coefficients. + pub(crate) d5220: f64, + /// Half-day resonance coefficients. + pub(crate) d5232: f64, + /// Half-day resonance coefficients. + pub(crate) d5421: f64, + /// Half-day resonance coefficients. + pub(crate) d5433: f64, + /// Resonance angle rate. + pub(crate) xfact: f64, + /// Resonance angle at the epoch, in radians. + pub(crate) xlamo: f64, +} + +impl DeepSpace { + /// The all zero value used by near-Earth orbits. + pub(crate) const ZERO: Self = Self { + e3: 0.0, + ee2: 0.0, + se2: 0.0, + se3: 0.0, + si2: 0.0, + si3: 0.0, + sl2: 0.0, + sl3: 0.0, + sl4: 0.0, + sgh2: 0.0, + sgh3: 0.0, + sgh4: 0.0, + sh2: 0.0, + sh3: 0.0, + xgh2: 0.0, + xgh3: 0.0, + xgh4: 0.0, + xh2: 0.0, + xh3: 0.0, + xi2: 0.0, + xi3: 0.0, + xl2: 0.0, + xl3: 0.0, + xl4: 0.0, + zmol: 0.0, + zmos: 0.0, + peo: 0.0, + pinco: 0.0, + plo: 0.0, + pgho: 0.0, + pho: 0.0, + irez: 0, + dedt: 0.0, + didt: 0.0, + dmdt: 0.0, + dnodt: 0.0, + domdt: 0.0, + del1: 0.0, + del2: 0.0, + del3: 0.0, + d2201: 0.0, + d2211: 0.0, + d3210: 0.0, + d3222: 0.0, + d4410: 0.0, + d4422: 0.0, + d5220: 0.0, + d5232: 0.0, + d5421: 0.0, + d5433: 0.0, + xfact: 0.0, + xlamo: 0.0, + }; +} + +/// The result of applying the long period periodic perturbations. +#[derive(Debug, Clone, Copy, PartialEq)] +pub(crate) struct PerturbedElements { + /// Eccentricity, dimensionless. + pub(crate) eccentricity: f64, + /// Inclination, in radians. + pub(crate) inclination: f64, + /// Right ascension of the ascending node, in radians. + pub(crate) node: f64, + /// Argument of perigee, in radians. + pub(crate) argument: f64, + /// Mean anomaly, in radians. + pub(crate) mean_anomaly: f64, +} + +/// The mean elements after the deep space secular and resonance terms. +#[derive(Debug, Clone, Copy, PartialEq)] +pub(crate) struct DeepSpaceState { + /// Eccentricity, dimensionless. + pub(crate) eccentricity: f64, + /// Argument of perigee, in radians. + pub(crate) argument: f64, + /// Inclination, in radians. + pub(crate) inclination: f64, + /// Mean anomaly, in radians. + pub(crate) mean_anomaly: f64, + /// Right ascension of the ascending node, in radians. + pub(crate) node: f64, + /// Mean motion, in radians per minute. + pub(crate) mean_motion: f64, + /// The angle of the resonance integrator at the propagated time. + pub(crate) atime: f64, + /// The resonance angle at the propagated time, in radians. + pub(crate) xli: f64, + /// The resonance mean motion at the propagated time. + pub(crate) xni: f64, +} + +/// Reduces a value into `[0, 2 pi)` by taking the positive remainder. +fn remainder_positive(value: f64) -> f64 { + value.rem_euclid(TWO_PI) +} + +/// Reduces a value keeping the sign of the dividend. +/// +/// The reference uses this convention for the right ascension, where a +/// negative result is meaningful, and it differs from a positive remainder +/// for negative inputs. +fn remainder_signed(value: f64) -> f64 { + if value >= 0.0 { + value % TWO_PI + } else { + -((-value) % TWO_PI) + } +} + +/// Computes the deep space coefficients for a satellite. +pub(crate) fn initialise(satellite: &Satellite) -> DeepSpace { + let lunisolar = lunisolar_coefficients(satellite); + let mut deep = DeepSpace::ZERO; + + deep.zmol = lunisolar.zmol; + deep.zmos = lunisolar.zmos; + deep.se2 = 2.0 * lunisolar.ss1 * lunisolar.ss6; + deep.se3 = 2.0 * lunisolar.ss1 * lunisolar.ss7; + deep.si2 = 2.0 * lunisolar.ss2 * lunisolar.sz12; + deep.si3 = 2.0 * lunisolar.ss2 * (lunisolar.sz13 - lunisolar.sz11); + deep.sl2 = -2.0 * lunisolar.ss3 * lunisolar.sz2; + deep.sl3 = -2.0 * lunisolar.ss3 * (lunisolar.sz3 - lunisolar.sz1); + deep.sl4 = -2.0 * lunisolar.ss3 * (-21.0 - 9.0 * lunisolar.emsq) * ZES; + deep.sgh2 = 2.0 * lunisolar.ss4 * lunisolar.sz32; + deep.sgh3 = 2.0 * lunisolar.ss4 * (lunisolar.sz33 - lunisolar.sz31); + deep.sgh4 = -18.0 * lunisolar.ss4 * ZES; + deep.sh2 = -2.0 * lunisolar.ss2 * lunisolar.sz22; + deep.sh3 = -2.0 * lunisolar.ss2 * (lunisolar.sz23 - lunisolar.sz21); + deep.ee2 = 2.0 * lunisolar.s1 * lunisolar.s6; + deep.e3 = 2.0 * lunisolar.s1 * lunisolar.s7; + deep.xi2 = 2.0 * lunisolar.s2 * lunisolar.z12; + deep.xi3 = 2.0 * lunisolar.s2 * (lunisolar.z13 - lunisolar.z11); + deep.xl2 = -2.0 * lunisolar.s3 * lunisolar.z2; + deep.xl3 = -2.0 * lunisolar.s3 * (lunisolar.z3 - lunisolar.z1); + deep.xl4 = -2.0 * lunisolar.s3 * (-21.0 - 9.0 * lunisolar.emsq) * ZEL; + deep.xgh2 = 2.0 * lunisolar.s4 * lunisolar.z32; + deep.xgh3 = 2.0 * lunisolar.s4 * (lunisolar.z33 - lunisolar.z31); + deep.xgh4 = -18.0 * lunisolar.s4 * ZEL; + deep.xh2 = -2.0 * lunisolar.s2 * lunisolar.z22; + deep.xh3 = -2.0 * lunisolar.s2 * (lunisolar.z23 - lunisolar.z21); + + initialise_rates(satellite, &lunisolar, &mut deep); + initialise_resonance(satellite, &lunisolar, &mut deep); + + deep +} + +/// Evaluates the luni-solar series at the epoch. +/// +/// The series is evaluated twice with different orientations, once aligned +/// with the Moon and once with the Sun. Both sets of results are needed +/// because the final coefficients pair the two. +fn lunisolar_coefficients(satellite: &Satellite) -> Lunisolar { + const C1SS: f64 = 2.986_479_7e-6; + const C1L: f64 = 4.796_806_5e-7; + + let ep = satellite.ecco; + let argpp = satellite.argpo; + let inclp = satellite.inclo; + let nodep = satellite.nodeo; + let np = satellite.no_unkozai; + + // Days counted from the start of 1900. The reference adds 18 261.5 to a + // day count from 1950, which is the same origin. + let day = satellite.epoch_mjd - 15_019.5; + + let emsq = ep * ep; + let betasq = 1.0 - emsq; + let rtemsq = betasq.sqrt(); + let snodm = nodep.sin(); + let cnodm = nodep.cos(); + let sinomm = argpp.sin(); + let cosomm = argpp.cos(); + let sinim = inclp.sin(); + let cosim = inclp.cos(); + + let xnodce = remainder_positive(4.523_602_0 - 9.242_202_9e-4 * day); + let stem = xnodce.sin(); + let ctem = xnodce.cos(); + let zcosil = 0.913_751_64 - 0.035_680_96 * ctem; + let zsinil = (1.0 - zcosil * zcosil).sqrt(); + let zsinhl = 0.089_683_511 * stem / zsinil; + let zcoshl = (1.0 - zsinhl * zsinhl).sqrt(); + let gam = 5.835_151_4 + 0.001_944_368_0 * day; + let zx = 0.397_854_16 * stem / zsinil; + let zy = zcoshl * ctem + 0.917_448_67 * zsinhl * stem; + let zx = gam + zx.atan2(zy) - xnodce; + let zcosgl = zx.cos(); + let zsingl = zx.sin(); + + let mut zcosg = 0.194_590_5; + let mut zsing = -0.980_884_58; + let mut zcosi = 0.917_448_67; + let mut zsini = 0.397_854_16; + let mut zcosh = cnodm; + let mut zsinh = snodm; + let mut cc = C1SS; + let xnoi = 1.0 / np; + + // Values from the first pass, for the Moon. + let (mut ss1, mut ss2, mut ss3, mut ss4, mut ss5, mut ss6, mut ss7) = + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + let ( + mut sz1, + mut sz2, + mut sz3, + mut sz11, + mut sz12, + mut sz13, + mut sz21, + mut sz22, + mut sz23, + mut sz31, + mut sz32, + mut sz33, + ) = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + // Values from the second pass, for the Sun. + let (mut s1, mut s2, mut s3, mut s4, mut s5, mut s6, mut s7) = + (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + let ( + mut z1, + mut z2, + mut z3, + mut z11, + mut z12, + mut z13, + mut z21, + mut z22, + mut z23, + mut z31, + mut z32, + mut z33, + ) = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + for pass in 0..2 { + let a1 = zcosg * zcosh + zsing * zcosi * zsinh; + let a3 = -zsing * zcosh + zcosg * zcosi * zsinh; + let a7 = -zcosg * zsinh + zsing * zcosi * zcosh; + let a8 = zsing * zsini; + let a9 = zsing * zsinh + zcosg * zcosi * zcosh; + let a10 = zcosg * zsini; + let a2 = cosim * a7 + sinim * a8; + let a4 = cosim * a9 + sinim * a10; + let a5 = -sinim * a7 + cosim * a8; + let a6 = -sinim * a9 + cosim * a10; + + let x1 = a1 * cosomm + a2 * sinomm; + let x2 = a3 * cosomm + a4 * sinomm; + let x3 = -a1 * sinomm + a2 * cosomm; + let x4 = -a3 * sinomm + a4 * cosomm; + let x5 = a5 * sinomm; + let x6 = a6 * sinomm; + let x7 = a5 * cosomm; + let x8 = a6 * cosomm; + + let z31p = 12.0 * x1 * x1 - 3.0 * x3 * x3; + let z32p = 24.0 * x1 * x2 - 6.0 * x3 * x4; + let z33p = 12.0 * x2 * x2 - 3.0 * x4 * x4; + let mut z1p = 3.0 * (a1 * a1 + a2 * a2) + z31p * emsq; + let mut z2p = 6.0 * (a1 * a3 + a2 * a4) + z32p * emsq; + let mut z3p = 3.0 * (a3 * a3 + a4 * a4) + z33p * emsq; + let z11p = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7 - 6.0 * x3 * x5); + let z12p = -6.0 * (a1 * a6 + a3 * a5) + + emsq * (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); + let z13p = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); + let z21p = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); + let z22p = 6.0 * (a4 * a5 + a2 * a6) + + emsq * (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); + let z23p = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); + // The reference doubles each of these and adds the sub-expression + // multiplied by `1 - e^2`. + z1p = z1p + z1p + betasq * z31p; + z2p = z2p + z2p + betasq * z32p; + z3p = z3p + z3p + betasq * z33p; + + let s3p = cc * xnoi; + let s2p = -0.5 * s3p / rtemsq; + let s4p = s3p * rtemsq; + let s1p = -15.0 * ep * s4p; + let s5p = x1 * x3 + x2 * x4; + let s6p = x2 * x3 + x1 * x4; + let s7p = x2 * x4 - x1 * x3; + + if pass == 0 { + ss1 = s1p; + ss2 = s2p; + ss3 = s3p; + ss4 = s4p; + ss5 = s5p; + ss6 = s6p; + ss7 = s7p; + sz1 = z1p; + sz2 = z2p; + sz3 = z3p; + sz11 = z11p; + sz12 = z12p; + sz13 = z13p; + sz21 = z21p; + sz22 = z22p; + sz23 = z23p; + sz31 = z31p; + sz32 = z32p; + sz33 = z33p; + + zcosg = zcosgl; + zsing = zsingl; + zcosi = zcosil; + zsini = zsinil; + zcosh = zcoshl * cnodm + zsinhl * snodm; + zsinh = snodm * zcoshl - cnodm * zsinhl; + cc = C1L; + } else { + s1 = s1p; + s2 = s2p; + s3 = s3p; + s4 = s4p; + s5 = s5p; + s6 = s6p; + s7 = s7p; + z1 = z1p; + z2 = z2p; + z3 = z3p; + z11 = z11p; + z12 = z12p; + z13 = z13p; + z21 = z21p; + z22 = z22p; + z23 = z23p; + z31 = z31p; + z32 = z32p; + z33 = z33p; + } + } + + Lunisolar { + ss1, + ss2, + ss3, + ss4, + ss5, + ss6, + ss7, + sz1, + sz2, + sz3, + sz11, + sz12, + sz13, + sz21, + sz22, + sz23, + sz31, + sz32, + sz33, + s1, + s2, + s3, + s4, + s5, + s6, + s7, + z1, + z2, + z3, + z11, + z12, + z13, + z21, + z22, + z23, + z31, + z32, + z33, + zmol: remainder_positive(4.719_967_2 + 0.229_971_50 * day - gam), + zmos: remainder_positive(6.256_583_7 + 0.017_201_977 * day), + emsq, + sinim, + cosim, + } +} + +/// Computes the secular rates of the mean elements. +fn initialise_rates(satellite: &Satellite, ls: &Lunisolar, deep: &mut DeepSpace) { + // The reference sets `shs` and `shll` to zero for inclinations within + // about 3 degrees of the equator or the pole, where the division by the + // sine of the inclination is ill conditioned. + let inclined = (5.235_987_7e-2..=PI - 5.235_987_7e-2).contains(&satellite.inclo); + + let ses = ls.ss1 * ZNS * ls.ss5; + let sis = ls.ss2 * ZNS * (ls.sz11 + ls.sz13); + let sls = -ZNS * ls.ss3 * (ls.sz1 + ls.sz3 - 14.0 - 6.0 * ls.emsq); + let sghs = ls.ss4 * ZNS * (ls.sz31 + ls.sz33 - 6.0); + let mut shs = -ZNS * ls.ss2 * (ls.sz21 + ls.sz23); + if !inclined { + shs = 0.0; + } + if ls.sinim != 0.0 { + shs /= ls.sinim; + } + let sgs = sghs - ls.cosim * shs; + + deep.dedt = ses + ls.s1 * ZNL * ls.s5; + deep.didt = sis + ls.s2 * ZNL * (ls.z11 + ls.z13); + deep.dmdt = sls - ZNL * ls.s3 * (ls.z1 + ls.z3 - 14.0 - 6.0 * ls.emsq); + + let sghl = ls.s4 * ZNL * (ls.z31 + ls.z33 - 6.0); + let mut shll = -ZNL * ls.s2 * (ls.z21 + ls.z23); + if !inclined { + shll = 0.0; + } + let mut domdt = sgs + sghl; + let mut dnodt = shs; + if ls.sinim != 0.0 { + domdt -= ls.cosim / ls.sinim * shll; + dnodt += shll / ls.sinim; + } + deep.domdt = domdt; + deep.dnodt = dnodt; +} + +/// Computes the resonance coefficients. +fn initialise_resonance(satellite: &Satellite, ls: &Lunisolar, deep: &mut DeepSpace) { + const Q22: f64 = 1.789_167_9e-6; + const Q31: f64 = 2.146_074_8e-6; + const Q33: f64 = 2.212_301_5e-7; + const ROOT22: f64 = 1.789_167_9e-6; + const ROOT32: f64 = 3.739_379_2e-7; + const ROOT44: f64 = 7.363_695_3e-9; + const ROOT52: f64 = 1.142_863_9e-7; + const ROOT54: f64 = 2.176_580_3e-9; + const X2O3: f64 = 2.0 / 3.0; + + let nm = satellite.no_unkozai; + let em = satellite.ecco; + let emsq = ls.emsq; + let sinim = ls.sinim; + let cosim = ls.cosim; + let mo = satellite.mo; + let nodeo = satellite.nodeo; + let argpo = satellite.argpo; + + // The synchronous resonance applies near one revolution per sidereal + // day and the half-day resonance near two, with a further restriction on + // the eccentricity. + let mut irez = 0; + if 0.003_490_658_5 < nm && nm < 0.005_235_987_7 { + irez = 1; + } + if (8.26e-3..=9.24e-3).contains(&nm) && em >= 0.5 { + irez = 2; + } + deep.irez = irez; + if irez == 0 { + return; + } + + let aonv = (nm / satellite.xke()).powf(X2O3); + let theta = remainder_positive(satellite.gsto); + + if irez == 2 { + // Half-day resonance. The eccentricity series is evaluated at the + // eccentricity from the element set, not the value carried through + // the secular rates. + let cosisq = cosim * cosim; + let emo = em; + let em = satellite.ecco; + let emsqo = emsq; + let emsq = em * em; + let eoc = em * emsq; + + let g201 = -0.306 - (em - 0.64) * 0.440; + let (g211, g310, g322, g410, g422, g520) = if em <= 0.65 { + ( + 3.616 - 13.2470 * em + 16.2900 * emsq, + -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc, + -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc, + -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc, + -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc, + -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc, + ) + } else { + let g520 = if em > 0.715 { + -5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc + } else { + 1464.74 - 4664.75 * em + 3763.64 * emsq + }; + ( + -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc, + -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc, + -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc, + -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc, + -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc, + g520, + ) + }; + let (g533, g521, g532) = if em < 0.7 { + ( + -919.227_70 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc, + -822.710_72 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc, + -853.666_00 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc, + ) + } else { + ( + -37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc, + -51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc, + -40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc, + ) + }; + + let sini2 = sinim * sinim; + let f220 = 0.75 * (1.0 + 2.0 * cosim + cosisq); + let f221 = 1.5 * sini2; + let f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); + let f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); + let f441 = 35.0 * sini2 * f220; + let f442 = 39.375_0 * sini2 * sini2; + let f522 = 9.843_75 + * sinim + * (sini2 * (1.0 - 2.0 * cosim - 5.0 * cosisq) + + 0.333_333_33 * (-2.0 + 4.0 * cosim + 6.0 * cosisq)); + let f523 = sinim + * (4.921_875_12 * sini2 * (-2.0 - 4.0 * cosim + 10.0 * cosisq) + + 6.562_500_12 * (1.0 + 2.0 * cosim - 3.0 * cosisq)); + let f542 = 29.531_25 + * sinim + * (2.0 - 8.0 * cosim + cosisq * (-12.0 + 8.0 * cosim + 10.0 * cosisq)); + let f543 = 29.531_25 + * sinim + * (-2.0 - 8.0 * cosim + cosisq * (12.0 + 8.0 * cosim - 10.0 * cosisq)); + + let xno2 = nm * nm; + let ainv2 = aonv * aonv; + let mut temp1 = 3.0 * xno2 * ainv2; + let mut temp = temp1 * ROOT22; + deep.d2201 = temp * f220 * g201; + deep.d2211 = temp * f221 * g211; + temp1 *= aonv; + temp = temp1 * ROOT32; + deep.d3210 = temp * f321 * g310; + deep.d3222 = temp * f322 * g322; + temp1 *= aonv; + temp = 2.0 * temp1 * ROOT44; + deep.d4410 = temp * f441 * g410; + deep.d4422 = temp * f442 * g422; + temp1 *= aonv; + temp = temp1 * ROOT52; + deep.d5220 = temp * f522 * g520; + deep.d5232 = temp * f523 * g532; + temp = 2.0 * temp1 * ROOT54; + deep.d5421 = temp * f542 * g521; + deep.d5433 = temp * f543 * g533; + + deep.xlamo = remainder_positive(mo + nodeo + nodeo - theta - theta); + deep.xfact = + satellite.mdot + deep.dmdt + 2.0 * (satellite.nodedot + deep.dnodt - RPTIM) - nm; + + let _ = (emo, emsqo); + } else { + // Synchronous resonance. + let g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); + let g310 = 1.0 + 2.0 * emsq; + let g300 = 1.0 + emsq * (-6.0 + 6.609_37 * emsq); + let f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); + let f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); + let f330 = 1.0 + cosim; + let f330 = 1.875 * f330 * f330 * f330; + let mut del1 = 3.0 * nm * nm * aonv * aonv; + let del2 = 2.0 * del1 * f220 * g200 * Q22; + let del3 = 3.0 * del1 * f330 * g300 * Q33 * aonv; + del1 = del1 * f311 * g310 * Q31 * aonv; + + deep.del1 = del1; + deep.del2 = del2; + deep.del3 = del3; + deep.xlamo = remainder_positive(mo + nodeo + argpo - theta); + deep.xfact = satellite.mdot + satellite.argpdot + satellite.nodedot - RPTIM + + deep.dmdt + + deep.domdt + + deep.dnodt + - nm; + } +} + +/// Applies the deep space secular and resonance terms to the mean elements. +#[allow(clippy::too_many_arguments)] +pub(crate) fn dspace( + satellite: &Satellite, + minutes: f64, + eccentricity: f64, + argument: f64, + inclination: f64, + mean_anomaly: f64, + node: f64, + mean_motion: f64, +) -> DeepSpaceState { + let deep = &satellite.ds; + let no = satellite.no_unkozai; + + let em = eccentricity + deep.dedt * minutes; + let inclm = inclination + deep.didt * minutes; + let argpm = argument + deep.domdt * minutes; + let nodem = node + deep.dnodt * minutes; + let mut mm = mean_anomaly + deep.dmdt * minutes; + let mut nm = mean_motion; + + let theta = remainder_positive(satellite.gsto + minutes * RPTIM); + + // The resonance is integrated in fixed 720 minute steps counted from + // the epoch. Starting from the epoch on every call rather than carrying + // the state between calls keeps propagation free of hidden state, and + // the result is the same because the step boundaries do not depend on + // the requested time. + let mut atime = 0.0; + let mut xli = deep.xlamo; + let mut xni = no; + + if deep.irez != 0 { + let delta_step = if minutes > 0.0 { + STEP_POSITIVE + } else { + STEP_NEGATIVE + }; + + let mut xndt; + let mut xnddt; + let mut xldot; + loop { + let (derivative, derivative_derivative) = resonance_derivatives( + deep, + satellite.argpo, + satellite.argpdot, + atime, + xli, + xni, + deep.irez, + ); + xndt = derivative; + xldot = xni + deep.xfact; + xnddt = derivative_derivative * xldot; + + if (minutes - atime).abs() >= STEP_POSITIVE { + xli += xldot * delta_step + xndt * STEP_SQUARED; + xni += xndt * delta_step + xnddt * STEP_SQUARED; + atime += delta_step; + } else { + break; + } + } + + let ft = minutes - atime; + nm = xni + xndt * ft + xnddt * ft * ft * 0.5; + let xl = xli + xldot * ft + xndt * ft * ft * 0.5; + if deep.irez != 1 { + mm = xl - 2.0 * nodem + 2.0 * theta; + } else { + mm = xl - nodem - argpm + theta; + } + nm = no + (nm - no); + } + + DeepSpaceState { + eccentricity: em, + argument: argpm, + inclination: inclm, + mean_anomaly: mm, + node: nodem, + mean_motion: nm, + atime, + xli, + xni, + } +} + +/// Returns the first and second derivatives of the resonance angle. +/// +/// The two resonance cases use different series. The synchronous case, +/// `irez == 1`, uses three terms that depend only on the angle, while the +/// half-day case uses ten terms that also depend on the argument of perigee. +fn resonance_derivatives( + deep: &DeepSpace, + argpo: f64, + argpdot: f64, + atime: f64, + xli: f64, + _xni: f64, + irez: i32, +) -> (f64, f64) { + const FASX2: f64 = 0.131_309_08; + const FASX4: f64 = 2.884_319_8; + const FASX6: f64 = 0.374_480_87; + const G22: f64 = 5.768_639_6; + const G32: f64 = 0.952_408_98; + const G44: f64 = 1.801_499_8; + const G52: f64 = 1.050_833_0; + const G54: f64 = 4.410_889_8; + + if irez != 2 { + let xndt = deep.del1 * (xli - FASX2).sin() + + deep.del2 * (2.0 * (xli - FASX4)).sin() + + deep.del3 * (3.0 * (xli - FASX6)).sin(); + let xnddt = deep.del1 * (xli - FASX2).cos() + + 2.0 * deep.del2 * (2.0 * (xli - FASX4)).cos() + + 3.0 * deep.del3 * (3.0 * (xli - FASX6)).cos(); + (xndt, xnddt) + } else { + let xomi = argpo + argpdot * atime; + let x2omi = xomi + xomi; + let x2li = xli + xli; + let xndt = deep.d2201 * (x2omi + xli - G22).sin() + + deep.d2211 * (xli - G22).sin() + + deep.d3210 * (xomi + xli - G32).sin() + + deep.d3222 * (-xomi + xli - G32).sin() + + deep.d4410 * (x2omi + x2li - G44).sin() + + deep.d4422 * (x2li - G44).sin() + + deep.d5220 * (xomi + xli - G52).sin() + + deep.d5232 * (-xomi + xli - G52).sin() + + deep.d5421 * (xomi + x2li - G54).sin() + + deep.d5433 * (-xomi + x2li - G54).sin(); + let xnddt = deep.d2201 * (x2omi + xli - G22).cos() + + deep.d2211 * (xli - G22).cos() + + deep.d3210 * (xomi + xli - G32).cos() + + deep.d3222 * (-xomi + xli - G32).cos() + + deep.d5220 * (xomi + xli - G52).cos() + + deep.d5232 * (-xomi + xli - G52).cos() + + 2.0 + * (deep.d4410 * (x2omi + x2li - G44).cos() + + deep.d4422 * (x2li - G44).cos() + + deep.d5421 * (xomi + x2li - G54).cos() + + deep.d5433 * (-xomi + x2li - G54).cos()); + (xndt, xnddt) + } +} + +/// Applies the deep space long period periodic perturbations. +/// +/// The terms are evaluated on the solar and lunar mean anomalies at the +/// requested time, except during initialisation, where they are evaluated at +/// the epoch and stored as the reference values that later evaluations +/// subtract. +#[allow(clippy::too_many_arguments)] +pub(crate) fn dpper( + satellite: &Satellite, + minutes: f64, + initialised: bool, + eccentricity: f64, + inclination: f64, + node: f64, + argument: f64, + mean_anomaly: f64, +) -> PerturbedElements { + let deep = &satellite.ds; + + let mut ep = eccentricity; + let mut inclp = inclination; + let mut nodep = node; + let mut argpp = argument; + let mut mp = mean_anomaly; + + let mut zm = deep.zmos + ZNS * minutes; + if initialised { + zm = deep.zmos; + } + let mut zf = zm + 2.0 * ZES * zm.sin(); + let sinzf = zf.sin(); + let f2 = 0.5 * sinzf * sinzf - 0.25; + let f3 = -0.5 * sinzf * zf.cos(); + let ses = deep.se2 * f2 + deep.se3 * f3; + let sis = deep.si2 * f2 + deep.si3 * f3; + let sls = deep.sl2 * f2 + deep.sl3 * f3 + deep.sl4 * sinzf; + let sghs = deep.sgh2 * f2 + deep.sgh3 * f3 + deep.sgh4 * sinzf; + let shs = deep.sh2 * f2 + deep.sh3 * f3; + + zm = deep.zmol + ZNL * minutes; + if initialised { + zm = deep.zmol; + } + zf = zm + 2.0 * ZEL * zm.sin(); + let sinzf = zf.sin(); + let f2 = 0.5 * sinzf * sinzf - 0.25; + let f3 = -0.5 * sinzf * zf.cos(); + let sel = deep.ee2 * f2 + deep.e3 * f3; + let sil = deep.xi2 * f2 + deep.xi3 * f3; + let sll = deep.xl2 * f2 + deep.xl3 * f3 + deep.xl4 * sinzf; + let sghl = deep.xgh2 * f2 + deep.xgh3 * f3 + deep.xgh4 * sinzf; + let shll = deep.xh2 * f2 + deep.xh3 * f3; + + let mut pe = ses + sel; + let mut pinc = sis + sil; + let mut pl = sls + sll; + let mut pgh = sghs + sghl; + let mut ph = shs + shll; + + if !initialised { + pe -= deep.peo; + pinc -= deep.pinco; + pl -= deep.plo; + pgh -= deep.pgho; + ph -= deep.pho; + inclp += pinc; + ep += pe; + let sinip = inclp.sin(); + let cosip = inclp.cos(); + + // The Lyddane modification is used at low inclination, where the + // node and the argument of perigee are individually ill conditioned. + // The threshold of 0.2 radians is about 11.5 degrees. The reference + // applies it against the perturbed inclination. + if inclp >= 0.2 { + ph /= sinip; + pgh -= cosip * ph; + argpp += pgh; + nodep += ph; + mp += pl; + } else { + let sinop = nodep.sin(); + let cosop = nodep.cos(); + let mut alfdp = sinip * sinop; + let mut betdp = sinip * cosop; + let dalf = ph * cosop + pinc * cosip * sinop; + let dbet = -ph * sinop + pinc * cosip * cosop; + alfdp += dalf; + betdp += dbet; + nodep = remainder_signed(nodep); + let xls = mp + argpp + pl + pgh + (cosip - pinc * sinip) * nodep; + let xnoh = nodep; + nodep = alfdp.atan2(betdp); + if (xnoh - nodep).abs() > PI { + if nodep < xnoh { + nodep += TWO_PI; + } else { + nodep -= TWO_PI; + } + } + mp += pl; + argpp = xls - mp - cosip * nodep; + } + } + + PerturbedElements { + eccentricity: ep, + inclination: inclp, + node: nodep, + argument: argpp, + mean_anomaly: mp, + } +} diff --git a/crates/orbit/apsis-sgp4/src/error.rs b/crates/orbit/apsis-sgp4/src/error.rs new file mode 100644 index 0000000..1d556a6 --- /dev/null +++ b/crates/orbit/apsis-sgp4/src/error.rs @@ -0,0 +1,70 @@ +//! Errors reported by the propagator. + +use core::fmt; + +/// The category of a propagation failure. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[non_exhaustive] +pub enum PropagationErrorKind { + /// The mean elements do not describe a satellite the model can + /// propagate. This includes an eccentricity at or above one, which is + /// not a closed orbit, and a mean motion that is not positive. + InvalidElements, + /// The satellite has decayed, meaning the drag term has brought it + /// below the surface of the Earth. + Decayed, + /// The propagation left the region where the model is defined. + OutOfRange, +} + +impl fmt::Display for PropagationErrorKind { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + let text = match self { + Self::InvalidElements => "mean elements are not propagatable", + Self::Decayed => "satellite has decayed", + Self::OutOfRange => "propagation is out of range", + }; + f.write_str(text) + } +} + +/// A failure while initialising or propagating a satellite. +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct PropagationError { + kind: PropagationErrorKind, + detail: Option, +} + +impl PropagationError { + /// Creates an error with the given category. + pub fn new(kind: PropagationErrorKind) -> Self { + Self { kind, detail: None } + } + + /// Attaches a description of the value that caused the failure. + pub fn with_detail(mut self, detail: impl Into) -> Self { + self.detail = Some(detail.into()); + self + } + + /// Returns the category of this failure. + pub fn kind(&self) -> PropagationErrorKind { + self.kind + } + + /// Returns the detail text attached to this error, if any. + pub fn detail(&self) -> Option<&str> { + self.detail.as_deref() + } +} + +impl fmt::Display for PropagationError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match &self.detail { + Some(detail) => write!(f, "{}: {detail}", self.kind), + None => write!(f, "{}", self.kind), + } + } +} + +impl core::error::Error for PropagationError {} diff --git a/crates/orbit/apsis-sgp4/src/init.rs b/crates/orbit/apsis-sgp4/src/init.rs new file mode 100644 index 0000000..4788129 --- /dev/null +++ b/crates/orbit/apsis-sgp4/src/init.rs @@ -0,0 +1,180 @@ +//! Initialisation of the propagator from a two-line element set. +//! +//! The element set gives mean elements in degrees, revolutions per day, and +//! a drag term. The propagator works in radians, radians per minute, and +//! Earth radii, so the initialisation converts the inputs and precomputes +//! every coefficient that depends only on the elements. +//! +//! The quantity `no_unkozai` is the mean motion after the correction that +//! removes the effect of the Kozai mean motion convention used in the +//! element set. The correction follows from the second zonal harmonic and +//! is solved by a fixed point iteration, because the corrected value appears +//! on both sides of the relation. +//! +//! Source: the correction and the coefficient definitions follow Vallado et +//! al., *Revisiting Spacetrack Report #3*, AIAA 2006-6753, 2006. + +use crate::error::{PropagationError, PropagationErrorKind}; +use crate::model::GravityModel; +use apsis_tle::TwoLineElement; +use core::f64::consts::PI; + +/// The number of minutes in a day. +const MINUTES_PER_DAY: f64 = 1_440.0; + +/// The number of radians in a revolution. +const TWO_PI: f64 = 2.0 * PI; + +/// The period, in minutes, at or above which the deep space branch is used. +/// +/// The theory states 225 minutes. Satellites below that period are handled +/// by the near-Earth branch, which is the case for all low Earth orbits. +const DEEP_SPACE_PERIOD_MINUTES: f64 = 225.0; + +/// The elements of a satellite in the units the propagator uses. +/// +/// Angles are in radians and mean motion is in radians per minute. Helpers +/// convert a [`TwoLineElement`] into this form. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Elements { + /// Epoch of the elements, expressed as a Modified Julian Date on the + /// scale the element set was written in. + pub epoch_mjd: f64, + /// `BSTAR` drag term, in inverse Earth radii. + pub bstar: f64, + /// Mean motion, in radians per minute. + pub mean_motion: f64, + /// Eccentricity, dimensionless. + pub eccentricity: f64, + /// Inclination, in radians. + pub inclination: f64, + /// Right ascension of the ascending node, in radians. + pub right_ascension: f64, + /// Argument of perigee, in radians. + pub argument_of_perigee: f64, + /// Mean anomaly, in radians. + pub mean_anomaly: f64, +} + +impl Elements { + /// Converts a parsed element set into the units the propagator uses. + /// + /// The epoch is taken as the UTC Modified Julian Date exactly as written + /// in the element set. The element set epoch is a UTC calendar instant, + /// but the propagator treats it as a uniform time origin, which is the + /// convention the theory was fitted under. Converting it to a uniform + /// scale first would shift the epoch by the leap second offset, about + /// 33 seconds, and that shift propagates into the solar and lunar series + /// of the deep space branch. + pub fn from_tle(element: &TwoLineElement) -> Option { + let epoch_mjd = element.epoch.mjd(apsis_time::TimeScale::Utc)?; + Some(Self { + epoch_mjd, + bstar: element.bstar, + mean_motion: element.mean_motion * TWO_PI / MINUTES_PER_DAY, + eccentricity: element.eccentricity, + inclination: element.inclination.to_radians(), + right_ascension: element.right_ascension.to_radians(), + argument_of_perigee: element.argument_of_perigee.to_radians(), + mean_anomaly: element.mean_anomaly.to_radians(), + }) + } +} + +/// Applies the Kozai to Brouwer mean motion correction. +/// +/// The mean motion in an element set is a Kozai mean motion, which the +/// theory must convert to a Brouwer mean motion before use. The relation is +/// +/// ```text +/// n0 = n / (1 + 1.5 k2 (3 cos^2 i - 1) / (a^2 sqrt(1 - e^2)) / a^2) +/// ``` +/// +/// written in the form used here as a fixed point iteration, because `a` +/// depends on `n0` through Kepler's third law. The iteration converges from +/// the unmodified mean motion in a few steps for every orbit that appears in +/// an element set. +/// Applies the Kozai to Brouwer mean motion correction. +/// +/// The mean motion in an element set is a Kozai mean motion, which the +/// theory must convert to a Brouwer mean motion before use. Writing `no` for +/// the element set value and `xke` for the Kepler constant, the reference +/// implementation evaluates +/// +/// ```text +/// ak = (xke / no)^(2/3) +/// d1 = 0.75 j2 (3 cos^2 i - 1) / (sqrt(1 - e^2) (1 - e^2)) +/// del = d1 / ak^2 +/// adel = ak (1 - del^2 - del (1/3 + 134 del^2 / 81)) +/// del = d1 / adel^2 +/// n0 = no / (1 + del) +/// ``` +/// +/// The second evaluation of `del` uses `adel` rather than `ak`, which is +/// what makes the correction consistent with the semi-major axis implied by +/// the corrected mean motion. +/// +/// Source: Vallado et al., *Revisiting Spacetrack Report #3*, AIAA +/// 2006-6753, 2006. +pub fn unkozai_mean_motion( + mean_motion: f64, + eccentricity: f64, + inclination: f64, + model: GravityModel, +) -> f64 { + let xke = model.xke(); + let j2 = model.j2; + let cosio = inclination.cos(); + let cosio2 = cosio * cosio; + let omeosq = 1.0 - eccentricity * eccentricity; + let rteosq = omeosq.sqrt(); + + let ak = (xke / mean_motion).powf(2.0 / 3.0); + let d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); + let mut delta = d1 / (ak * ak); + let adel = ak * (1.0 - delta * delta - delta * (1.0 / 3.0 + 134.0 * delta * delta / 81.0)); + delta = d1 / (adel * adel); + mean_motion / (1.0 + delta) +} + +/// Validates the mean elements before initialisation. +/// +/// Returns an error when the orbit cannot be propagated, for example when +/// the eccentricity does not describe a closed orbit or the mean motion is +/// not positive. +pub fn validate(elements: &Elements) -> Result<(), PropagationError> { + if !elements.mean_motion.is_finite() || elements.mean_motion <= 0.0 { + return Err(PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail(format!("mean motion is {}", elements.mean_motion))); + } + if !elements.eccentricity.is_finite() + || elements.eccentricity < 0.0 + || elements.eccentricity >= 1.0 + { + return Err(PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail(format!("eccentricity is {}", elements.eccentricity))); + } + for (name, value) in [ + ("inclination", elements.inclination), + ("right ascension", elements.right_ascension), + ("argument of perigee", elements.argument_of_perigee), + ("mean anomaly", elements.mean_anomaly), + ] { + if !value.is_finite() { + return Err(PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail(format!("{name} is {value}"))); + } + } + if !elements.bstar.is_finite() { + return Err(PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail(format!("bstar is {}", elements.bstar))); + } + Ok(()) +} + +/// Returns `true` when the elements describe a deep space orbit. +/// +/// The test is on the orbital period, which follows from the mean motion. +pub fn is_deep_space(mean_motion: f64) -> bool { + TWO_PI / mean_motion >= DEEP_SPACE_PERIOD_MINUTES +} diff --git a/crates/orbit/apsis-sgp4/src/lib.rs b/crates/orbit/apsis-sgp4/src/lib.rs index 0e34a08..a3bfcb8 100644 --- a/crates/orbit/apsis-sgp4/src/lib.rs +++ b/crates/orbit/apsis-sgp4/src/lib.rs @@ -1 +1,70 @@ -//! SGP4/SDP4 analytical orbit propagator. +//! The SGP4 and SDP4 analytical orbit propagator. +//! +//! The propagator takes the mean orbital elements of a two-line element set +//! and returns the position and velocity of the satellite at a requested +//! instant. It is the model that the element sets are fitted to, so it is +//! the correct way to use them; a general purpose numerical integrator +//! given the same elements would drift away from the published positions. +//! +//! Two branches exist and the propagator selects between them by orbital +//! period. A satellite with a period below 225 minutes is propagated with +//! SGP4, which models the secular effect of the Earth's oblateness and a +//! drag term. A satellite at or above that period is propagated with SDP4, +//! which adds the resonance and the luni-solar perturbations that dominate +//! at that altitude. +//! +//! Inputs and outputs: +//! +//! - Elements are the mean elements from [`apsis_tle::TwoLineElement`]. +//! Angles in the element set are in degrees and mean motion is in +//! revolutions per day; this crate converts to radians and radians per +//! minute internally. +//! - The output is expressed in the True Equator Mean Equinox (TEME) frame, +//! with position in kilometers and velocity in kilometers per second. TEME +//! is the frame the model defines; converting it to an Earth-fixed frame +//! requires the Earth rotation and the equation of the equinoxes and is +//! not part of this crate. +//! - Time is measured from the element set epoch, in minutes. +//! +//! Source: the algorithm follows Vallado et al., *Revisiting Spacetrack +//! Report #3*, AIAA 2006-6753, 2006, and the revised implementation +//! distributed with that paper. +//! +//! ``` +//! use apsis_sgp4::{GravityModel, Satellite}; +//! use apsis_tle::TwoLineElement; +//! +//! let text = concat!( +//! "1 25544U 98067A 08264.51782528 -.00002182 00000-0 -11606-4 0 2927\r\n", +//! "2 25544 51.6416 247.4627 0006703 130.5360 325.0288 15.72125391563537\r\n", +//! ); +//! let element = TwoLineElement::parse(text).expect("sample is well formed"); +//! let satellite = Satellite::from_element(&element, GravityModel::WGS72) +//! .expect("sample elements are propagatable"); +//! +//! // Propagate to the epoch of the elements. +//! let state = satellite.propagate_minutes(0.0).expect("epoch state exists"); +//! // The position of a low Earth satellite is a few thousand kilometers +//! // from the center of the Earth. +//! assert!(state.position.norm() > 6_600.0); +//! assert!(state.position.norm() < 7_200.0); +//! ``` + +#![forbid(unsafe_code)] +#![deny(missing_docs)] + +mod deep_space; +mod error; +mod init; +mod model; +mod propagate; +mod satellite; +mod state; + +pub use error::{PropagationError, PropagationErrorKind}; +pub use init::Elements; +pub use model::GravityModel; +pub use satellite::Satellite; +pub use state::State; + +pub use apsis_tle::TwoLineElement; diff --git a/crates/orbit/apsis-sgp4/src/model.rs b/crates/orbit/apsis-sgp4/src/model.rs new file mode 100644 index 0000000..8cccc7a --- /dev/null +++ b/crates/orbit/apsis-sgp4/src/model.rs @@ -0,0 +1,108 @@ +//! Gravity models used by the propagator. +//! +//! The propagator needs four numbers that describe the Earth: the +//! gravitational parameter, the equatorial radius, and the second and third +//! zonal harmonics. The SGP4 model was fitted to the WGS 72 values and the +//! standard element sets distributed by Space-Track are intended to be used +//! with them, so WGS 72 is the default. +//! +//! WGS 84 is offered because elements fitted against it also exist. Mixing +//! models with element sets fitted against the other model shifts the +//! position by kilometers, so the choice should match the source of the +//! elements. +//! +//! The derived values `xke`, `tumin`, and `j3oj2` follow from the four +//! defining numbers and are computed once at construction. +//! +//! Sources: +//! +//! - WGS 72 values as used by the SGP4 reference implementation in +//! Vallado et al., *Revisiting Spacetrack Report #3*, AIAA 2006-6753, +//! appendix. The values match the WGS 72 gravity field. +//! - WGS 84 values: NIMA TR8350.2, third edition, 2000. + +/// A gravity model: the Earth constants used by the propagator. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct GravityModel { + /// Gravitational parameter, in cubic kilometers per second squared. + pub mu: f64, + /// Equatorial radius, in kilometers. + pub radius: f64, + /// Second zonal harmonic, dimensionless. + pub j2: f64, + /// Third zonal harmonic, dimensionless. + pub j3: f64, + /// Fourth zonal harmonic, dimensionless. + pub j4: f64, +} + +impl GravityModel { + /// The WGS 72 gravity model. + /// + /// This is the model the SGP4 theory was fitted to and the one that + /// matches the standard element sets. + pub const WGS72: Self = Self { + mu: 398_600.8, + radius: 6_378.135, + j2: 0.001_082_616, + j3: -0.000_002_538_81, + j4: -0.000_001_655_97, + }; + + /// The WGS 84 gravity model. + /// + /// Use this only with element sets fitted against WGS 84. + pub const WGS84: Self = Self { + mu: 398_600.5, + radius: 6_378.137, + j2: 0.001_082_629_89, + j3: -0.000_002_532_153_1, + j4: -0.000_001_610_988_4, + }; + + /// Returns the reciprocal of the square root of the gravitational + /// parameter, in seconds per kilometer to the three halves. + pub fn reciprocal_sqrt_mu(self) -> f64 { + 1.0 / self.mu.sqrt() + } + + /// Returns the square root of the gravitational parameter, in kilometers + /// to the three halves per second. + pub fn sqrt_mu(self) -> f64 { + self.mu.sqrt() + } + + /// Returns `xke`, the Kepler constant, in units of `er^1.5 / min`. + /// + /// The propagator works in minutes and Earth radii, so the gravitational + /// parameter is converted by + /// + /// ```text + /// xke = 60 / sqrt(radius^3 / mu) + /// ``` + /// + /// which is the angular rate, in radians per minute, of a circular orbit + /// of radius one Earth radius. + pub fn xke(self) -> f64 { + 60.0 / (self.radius * self.radius * self.radius / self.mu).sqrt() + } + + /// Returns the reciprocal of `xke`, in minutes. + /// + /// This is the time unit of the model: the period of the reference + /// circular orbit divided by two pi. + pub fn tumin(self) -> f64 { + 1.0 / self.xke() + } + + /// Returns `j3 / j2`, which appears as a single ratio in the theory. + pub fn j3_over_j2(self) -> f64 { + self.j3 / self.j2 + } + + /// Returns `2 * xke^2`, a term that appears in the mean motion + /// correction. + pub fn x2o3(self) -> f64 { + 2.0 / 3.0 + } +} diff --git a/crates/orbit/apsis-sgp4/src/propagate.rs b/crates/orbit/apsis-sgp4/src/propagate.rs new file mode 100644 index 0000000..1aea8a0 --- /dev/null +++ b/crates/orbit/apsis-sgp4/src/propagate.rs @@ -0,0 +1,299 @@ +//! The near-Earth propagation step. +//! +//! Given a satellite record and a time from epoch, this module evaluates the +//! secular and periodic terms of the SGP4 theory and returns the state in +//! the True Equator Mean Equinox frame. +//! +//! The mean elements advance as polynomials in time: the mean anomaly, +//! argument of perigee, and right ascension are linear, the right ascension +//! has an additional quadratic term, and the semi-major axis, eccentricity, +//! and mean anomaly pick up further drag terms for orbits above the +//! simplified threshold. The orbit is then solved for the eccentric anomaly +//! by Newton's method, and the short period terms are applied. +//! +//! Source: the sequence follows Vallado et al., *Revisiting Spacetrack +//! Report #3*, AIAA 2006-6753, 2006, and the reference implementation +//! distributed with that paper. + +use crate::error::{PropagationError, PropagationErrorKind}; +use crate::satellite::{Method, Satellite, TWO_PI}; +use crate::state::State; +use apsis_math::Vec3; + +/// The number of iterations allowed when solving for the eccentric anomaly. +const KEPLER_MAX_ITERATIONS: usize = 10; + +/// The convergence threshold on the eccentric anomaly correction. +const KEPLER_TOLERANCE: f64 = 1.0e-12; + +/// The largest correction applied in one Kepler step. +/// +/// Clamping keeps the iteration stable for the eccentric orbits that the +/// near circular approximation of the theory handles poorly. +const KEPLER_MAX_STEP: f64 = 0.95; + +/// The smallest eccentricity used after the drag terms are applied. +/// +/// A value that would round to zero makes several divisions in the short +/// period terms singular, so it is clamped to this floor. +const MINIMUM_ECCENTRICITY: f64 = 1.0e-6; + +/// The divisor used when the inclination is close to pi. +const INCLINATION_GUARD: f64 = 1.5e-12; + +/// Propagates a satellite by a number of minutes from its element epoch. +pub(crate) fn propagate(satellite: &Satellite, minutes: f64) -> Result { + let mean_anomaly = satellite.mo + satellite.mdot * minutes; + let secular_argument = satellite.argpo + satellite.argpdot * minutes; + let secular_node = satellite.nodeo + satellite.nodedot * minutes; + + let time_squared = minutes * minutes; + let mut node = secular_node + satellite.nodecf * time_squared; + + let mut tempa = 1.0 - satellite.cc1 * minutes; + let mut tempe = satellite.bstar * satellite.cc4 * minutes; + let mut templ = satellite.t2cof * time_squared; + + let mut argument = secular_argument; + let mut anomaly = mean_anomaly; + + if !satellite.isimp { + let delta_argument = satellite.omgcof * minutes; + let drag_anomaly = 1.0 + satellite.eta * mean_anomaly.cos(); + let delta_anomaly = + satellite.xmcof * (drag_anomaly * drag_anomaly * drag_anomaly - satellite.delmo); + let total = delta_argument + delta_anomaly; + anomaly = mean_anomaly + total; + argument = secular_argument - total; + let time_cubed = time_squared * minutes; + let time_fourth = time_cubed * minutes; + tempa -= + satellite.d2 * time_squared + satellite.d3 * time_cubed + satellite.d4 * time_fourth; + tempe += satellite.bstar * satellite.cc5 * (anomaly.sin() - satellite.sinmao); + templ = templ + + satellite.t3cof * time_cubed + + time_fourth * (satellite.t4cof + minutes * satellite.t5cof); + } + + let mut eccentricity = satellite.ecco; + let mut inclination = satellite.inclo; + let mut mean_motion = satellite.no_unkozai; + + // The deep space branch replaces the mean elements here. + if satellite.method == Method::DeepSpace { + let deep = crate::deep_space::dspace( + satellite, + minutes, + eccentricity, + argument, + inclination, + anomaly, + node, + mean_motion, + ); + eccentricity = deep.eccentricity; + argument = deep.argument; + inclination = deep.inclination; + anomaly = deep.mean_anomaly; + node = deep.node; + mean_motion = deep.mean_motion; + } + + if mean_motion <= 0.0 { + return Err(super::satellite::non_positive_mean_motion(mean_motion)); + } + + // Recover the semi-major axis including the drag scaling, then recompute + // the mean motion from it so the two stay consistent. + let semi_major = (satellite.xke() / mean_motion).powf(2.0 / 3.0) * tempa * tempa; + mean_motion = satellite.xke() / semi_major.powf(1.5); + + eccentricity -= tempe; + if eccentricity >= 1.0 || eccentricity < -0.001 { + return Err(PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail(format!("propagated eccentricity is {eccentricity}"))); + } + let eccentricity = eccentricity.max(MINIMUM_ECCENTRICITY); + + anomaly += satellite.no_unkozai * templ; + let mut longitude = anomaly + argument + node; + + // Reduce the angles to a single turn. The reference implementation uses + // a remainder that keeps the sign of the dividend for the right + // ascension and a positive remainder elsewhere; both are reproduced here + // because the eccentric anomaly iteration is sensitive to the branch. + node = if node >= 0.0 { + node % TWO_PI + } else { + -((-node) % TWO_PI) + }; + argument %= TWO_PI; + longitude %= TWO_PI; + anomaly = (longitude - argument - node) % TWO_PI; + + let mut perturbed_eccentricity = eccentricity; + let mut perturbed_inclination = inclination; + let mut perturbed_node = node; + let mut perturbed_argument = argument; + let mut perturbed_anomaly = anomaly; + let mut sin_perturbed = inclination.sin(); + let mut cos_perturbed = inclination.cos(); + + if satellite.method == Method::DeepSpace { + let result = crate::deep_space::dpper( + satellite, + minutes, + false, + perturbed_eccentricity, + perturbed_inclination, + perturbed_node, + perturbed_argument, + perturbed_anomaly, + ); + perturbed_eccentricity = result.eccentricity; + perturbed_inclination = result.inclination; + perturbed_node = result.node; + perturbed_argument = result.argument; + perturbed_anomaly = result.mean_anomaly; + + if perturbed_inclination < 0.0 { + perturbed_inclination = -perturbed_inclination; + perturbed_node += core::f64::consts::PI; + perturbed_argument -= core::f64::consts::PI; + } + if perturbed_eccentricity < 0.0 || perturbed_eccentricity > 1.0 { + return Err( + PropagationError::new(PropagationErrorKind::InvalidElements).with_detail(format!( + "perturbed eccentricity is {perturbed_eccentricity}" + )), + ); + } + + sin_perturbed = perturbed_inclination.sin(); + cos_perturbed = perturbed_inclination.cos(); + } + + // The `aycof` and `xlcof` used below come from the perturbed + // inclination in the deep space branch and from the epoch inclination + // otherwise. + let (aycof, xlcof) = if satellite.method == Method::DeepSpace { + let aycof = -0.5 * satellite.model.j3_over_j2() * sin_perturbed; + let xlcof = if (cos_perturbed + 1.0).abs() > INCLINATION_GUARD { + -0.25 * satellite.model.j3_over_j2() * sin_perturbed * (3.0 + 5.0 * cos_perturbed) + / (1.0 + cos_perturbed) + } else { + -0.25 * satellite.model.j3_over_j2() * sin_perturbed * (3.0 + 5.0 * cos_perturbed) + / INCLINATION_GUARD + }; + (aycof, xlcof) + } else { + (satellite.aycof, satellite.xlcof) + }; + + let axnl = perturbed_eccentricity * perturbed_argument.cos(); + let temp = 1.0 / (semi_major * (1.0 - perturbed_eccentricity * perturbed_eccentricity)); + let aynl = perturbed_eccentricity * perturbed_argument.sin() + temp * aycof; + let xl = perturbed_anomaly + perturbed_argument + perturbed_node + temp * xlcof * axnl; + + let u = (xl - perturbed_node) % TWO_PI; + + // Solve Kepler's equation by Newton's method. + let mut eo1 = u; + let (mut sineo1, mut coseo1); + let mut iterations = 0; + loop { + sineo1 = eo1.sin(); + coseo1 = eo1.cos(); + let denominator = 1.0 - coseo1 * axnl - sineo1 * aynl; + let mut correction = (u - aynl * coseo1 + axnl * sineo1 - eo1) / denominator; + if correction.abs() >= KEPLER_MAX_STEP { + correction = if correction > 0.0 { + KEPLER_MAX_STEP + } else { + -KEPLER_MAX_STEP + }; + } + eo1 += correction; + iterations += 1; + if correction.abs() < KEPLER_TOLERANCE || iterations >= KEPLER_MAX_ITERATIONS { + break; + } + } + sineo1 = eo1.sin(); + coseo1 = eo1.cos(); + + let ecose = axnl * coseo1 + aynl * sineo1; + let esine = axnl * sineo1 - aynl * coseo1; + let el2 = axnl * axnl + aynl * aynl; + let pl = semi_major * (1.0 - el2); + if pl < 0.0 { + return Err(PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail(format!("semi-latus rectum is {pl}"))); + } + + let rl = semi_major * (1.0 - ecose); + let rdotl = semi_major.sqrt() * esine / rl; + let rvdotl = pl.sqrt() / rl; + let betal = (1.0 - el2).sqrt(); + let temp = esine / (1.0 + betal); + let sinu = semi_major / rl * (sineo1 - aynl - axnl * temp); + let cosu = semi_major / rl * (coseo1 - axnl + aynl * temp); + let mut su = sinu.atan2(cosu); + let sin2u = (cosu + cosu) * sinu; + let cos2u = 1.0 - 2.0 * sinu * sinu; + + let temp = 1.0 / pl; + let temp1 = 0.5 * satellite.model.j2 * temp; + let temp2 = temp1 * temp; + + let (con41, x1mth2, x7thm1) = if satellite.method == Method::DeepSpace { + let cosisq = cos_perturbed * cos_perturbed; + (3.0 * cosisq - 1.0, 1.0 - cosisq, 7.0 * cosisq - 1.0) + } else { + (satellite.con41, satellite.x1mth2, satellite.x7thm1) + }; + + let mrt = rl * (1.0 - 1.5 * temp2 * betal * con41) + 0.5 * temp1 * x1mth2 * cos2u; + su -= 0.25 * temp2 * x7thm1 * sin2u; + let xnode = perturbed_node + 1.5 * temp2 * cos_perturbed * sin2u; + let xinc = perturbed_inclination + 1.5 * temp2 * cos_perturbed * sin_perturbed * cos2u; + + let mvt = rdotl - mean_motion * temp1 * x1mth2 * sin2u / satellite.xke(); + let rvdot = rvdotl + mean_motion * temp1 * (x1mth2 * cos2u + 1.5 * con41) / satellite.xke(); + + let sinsu = su.sin(); + let cossu = su.cos(); + let snod = xnode.sin(); + let cnod = xnode.cos(); + let sini = xinc.sin(); + let cosi = xinc.cos(); + + let xmx = -snod * cosi; + let xmy = cnod * cosi; + let ux = xmx * sinsu + cnod * cossu; + let uy = xmy * sinsu + snod * cossu; + let uz = sini * sinsu; + let vx = xmx * cossu - cnod * sinsu; + let vy = xmy * cossu - snod * sinsu; + let vz = sini * cossu; + + let radius = mrt * satellite.model.radius; + let position = Vec3::new(radius * ux, radius * uy, radius * uz); + let velocity_scale = satellite.model.radius * satellite.xke() / 60.0; + let velocity = Vec3::new( + (mvt * ux + rvdot * vx) * velocity_scale, + (mvt * uy + rvdot * vy) * velocity_scale, + (mvt * uz + rvdot * vz) * velocity_scale, + ); + + if mrt < 1.0 { + return Err(PropagationError::new(PropagationErrorKind::Decayed) + .with_detail(format!("radius is {mrt} Earth radii"))); + } + + Ok(State { + position, + velocity, + semi_major_axis: semi_major * satellite.model.radius, + }) +} diff --git a/crates/orbit/apsis-sgp4/src/satellite.rs b/crates/orbit/apsis-sgp4/src/satellite.rs new file mode 100644 index 0000000..f8f4835 --- /dev/null +++ b/crates/orbit/apsis-sgp4/src/satellite.rs @@ -0,0 +1,416 @@ +//! The satellite record and its initialisation from mean elements. + +use crate::error::{PropagationError, PropagationErrorKind}; +use crate::init::{self, Elements}; +use crate::model::GravityModel; +use crate::state::State; +use apsis_tle::TwoLineElement; +use core::f64::consts::PI; + +/// The number of radians in a revolution. +pub(crate) const TWO_PI: f64 = 2.0 * PI; + +/// The number of minutes in a day. +pub(crate) const MINUTES_PER_DAY: f64 = 1_440.0; + +/// Whether the near-Earth or the deep space branch applies. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub(crate) enum Method { + /// Period below 225 minutes. + NearEarth, + /// Period at or above 225 minutes. + DeepSpace, +} + +/// A satellite prepared for propagation. +/// +/// Construct one with [`Satellite::from_element`], then call +/// [`Satellite::propagate_minutes`] or [`Satellite::propagate_at`] for each +/// instant of interest. The initialisation work, including the mean motion +/// correction and every coefficient that depends only on the elements, is +/// done once at construction. +#[derive(Debug, Clone)] +pub struct Satellite { + /// The model this satellite was initialised against. + pub(crate) model: GravityModel, + /// Corrected mean motion, in radians per minute. + pub(crate) no_unkozai: f64, + /// Eccentricity. + pub(crate) ecco: f64, + /// Inclination, in radians. + pub(crate) inclo: f64, + /// Right ascension of the ascending node, in radians. + pub(crate) nodeo: f64, + /// Argument of perigee, in radians. + pub(crate) argpo: f64, + /// Mean anomaly, in radians. + pub(crate) mo: f64, + /// `BSTAR` drag term, in inverse Earth radii. + pub(crate) bstar: f64, + /// Epoch as a Modified Julian Date on the TAI scale. + pub(crate) epoch_mjd: f64, + /// Which branch of the theory applies. + pub(crate) method: Method, + /// Set when the short period terms are omitted. + pub(crate) isimp: bool, + + /// `3 cos^2 i - 1`. + pub(crate) con41: f64, + /// `1 - cos^2 i`. + pub(crate) x1mth2: f64, + /// `7 cos^2 i - 1`. + pub(crate) x7thm1: f64, + /// `-0.5 j3 / j2 sin i`. + pub(crate) aycof: f64, + /// The `j3` contribution to the argument of latitude. + pub(crate) xlcof: f64, + + /// First drag coefficient. + pub(crate) cc1: f64, + /// Fourth drag coefficient. + pub(crate) cc4: f64, + /// Fifth drag coefficient. + pub(crate) cc5: f64, + /// Second order drag coefficient. + pub(crate) d2: f64, + /// Third order drag coefficient. + pub(crate) d3: f64, + /// Fourth order drag coefficient. + pub(crate) d4: f64, + /// `(1 + eta cos M)^3` at epoch. + pub(crate) delmo: f64, + /// `eta`. + pub(crate) eta: f64, + /// Contribution of drag to the argument of perigee. + pub(crate) omgcof: f64, + /// `sin M` at epoch. + pub(crate) sinmao: f64, + /// Quadratic coefficient of the time polynomial. + pub(crate) t2cof: f64, + /// Cubic coefficient of the time polynomial. + pub(crate) t3cof: f64, + /// Quartic coefficient of the time polynomial. + pub(crate) t4cof: f64, + /// Quintic coefficient of the time polynomial. + pub(crate) t5cof: f64, + /// Contribution of drag to the mean anomaly. + pub(crate) xmcof: f64, + + /// Rate of change of the mean anomaly, in radians per minute. + pub(crate) mdot: f64, + /// Rate of change of the argument of perigee, in radians per minute. + pub(crate) argpdot: f64, + /// Rate of change of the right ascension, in radians per minute. + pub(crate) nodedot: f64, + /// Quadratic term of the right ascension. + pub(crate) nodecf: f64, + + /// Greenwich sidereal time at the epoch, in radians. + pub(crate) gsto: f64, + + /// Deep space resonance coefficients, all zero for near-Earth orbits. + pub(crate) ds: crate::deep_space::DeepSpace, +} + +impl Satellite { + /// Initialises a satellite from a parsed element set. + /// + /// Returns `None` when the mean elements do not describe an orbit the + /// model can propagate, for example when the eccentricity is at least + /// one. A caller that needs the reason should use + /// [`Satellite::try_from_element`]. + pub fn from_element(element: &TwoLineElement, model: GravityModel) -> Option { + Self::try_from_element(element, model).ok() + } + + /// Initialises a satellite from a parsed element set. + /// + /// Returns an error describing which element is not propagatable when + /// the elements are rejected. + pub fn try_from_element( + element: &TwoLineElement, + model: GravityModel, + ) -> Result { + let elements = Elements::from_tle(element).ok_or_else(|| { + PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail("element set epoch is outside the leap second table") + })?; + Self::try_from_elements(&elements, model) + } + + /// Initialises a satellite from mean elements already in model units. + pub fn try_from_elements( + elements: &Elements, + model: GravityModel, + ) -> Result { + init::validate(elements)?; + Ok(Self::initialise(elements, model)) + } + + /// Returns the epoch of the elements as a Modified Julian Date on the + /// TAI scale. + pub fn epoch_mjd(&self) -> f64 { + self.epoch_mjd + } + + /// Returns `true` when the deep space branch applies. + pub fn is_deep_space(&self) -> bool { + self.method == Method::DeepSpace + } + + /// Returns the number of minutes from the element set epoch to the + /// given instant. + /// + /// The instant is given as a Modified Julian Date on the TAI scale, the + /// same scale as [`Satellite::epoch_mjd`]. + pub fn minutes_since_epoch(&self, tai_mjd: f64) -> f64 { + (tai_mjd - self.epoch_mjd) * MINUTES_PER_DAY + } + + /// Returns the state of the satellite at a given instant. + /// + /// The instant is a Modified Julian Date on the TAI scale. + pub fn propagate_at(&self, tai_mjd: f64) -> Result { + self.propagate_minutes(self.minutes_since_epoch(tai_mjd)) + } + + /// Returns the state of the satellite at a number of minutes from the + /// element set epoch. + /// + /// Negative values propagate backwards. The result is in the True + /// Equator Mean Equinox frame, with position in kilometers and velocity + /// in kilometers per second. + /// + /// Returns [`PropagationErrorKind::Decayed`](crate::PropagationErrorKind::Decayed) + /// when the propagated radius falls below one Earth radius, which the + /// model treats as the satellite having re-entered. + pub fn propagate_minutes(&self, minutes: f64) -> Result { + crate::propagate::propagate(self, minutes) + } + + /// Returns the Kepler constant of the model, in radians per minute per + /// Earth radius to the three halves. + pub fn xke(&self) -> f64 { + self.model.xke() + } +} + +impl Satellite { + /// Builds the record, including every coefficient that depends only on + /// the epoch elements. + fn initialise(elements: &Elements, model: GravityModel) -> Self { + let xke = model.xke(); + let j2 = model.j2; + let j4 = model.j4; + + let ecco = elements.eccentricity; + let inclo = elements.inclination; + let no_kozai = elements.mean_motion; + + let no_unkozai = init::unkozai_mean_motion(no_kozai, ecco, inclo, model); + let method = if init::is_deep_space(no_unkozai) { + Method::DeepSpace + } else { + Method::NearEarth + }; + + let cosio = inclo.cos(); + let sinio = inclo.sin(); + let cosio2 = cosio * cosio; + let cosio4 = cosio2 * cosio2; + let omeosq = 1.0 - ecco * ecco; + let rteosq = omeosq.sqrt(); + + let ao = (xke / no_unkozai).powf(2.0 / 3.0); + let po = ao * omeosq; + let posq = po * po; + let rp = ao * (1.0 - ecco); + let con42 = 1.0 - 5.0 * cosio2; + let con41 = -con42 - cosio2 - cosio2; + + // The aerodynamic parameters. `sfour` is the effective radius at + // which the atmosphere is treated as having constant density, and + // `qzms24` is the corresponding density scale raised to the fourth + // power. + let ss = 78.0 / model.radius + 1.0; + let qzms2t = ((120.0 - 78.0) / model.radius).powi(4); + + let mut isimp = false; + if rp < 220.0 / model.radius + 1.0 { + isimp = true; + } + let mut sfour = ss; + let mut qzms24 = qzms2t; + let perige = (rp - 1.0) * model.radius; + if perige < 156.0 { + sfour = perige - 78.0; + if perige < 98.0 { + sfour = 20.0; + } + qzms24 = ((120.0 - sfour) / model.radius).powi(4); + sfour = sfour / model.radius + 1.0; + } + + let pinvsq = 1.0 / posq; + let tsi = 1.0 / (ao - sfour); + let eta = ao * ecco * tsi; + let etasq = eta * eta; + let eeta = ecco * eta; + let psisq = (1.0 - etasq).abs(); + let coef = qzms24 * tsi.powi(4); + let coef1 = coef / psisq.powf(3.5); + + let cc2 = coef1 + * no_unkozai + * (ao * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) + + 0.375 * j2 * tsi / psisq * con41 * (8.0 + 3.0 * etasq * (8.0 + etasq))); + let cc1 = elements.bstar * cc2; + + let mut cc3 = 0.0; + if ecco > 1.0e-4 { + cc3 = -2.0 * coef * tsi * model.j3_over_j2() * no_unkozai * sinio / ecco; + } + let x1mth2 = 1.0 - cosio2; + let cc4 = 2.0 + * no_unkozai + * coef1 + * ao + * omeosq + * (eta * (2.0 + 0.5 * etasq) + ecco * (0.5 + 2.0 * etasq) + - j2 * tsi / (ao * psisq) + * (-3.0 * con41 * (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta)) + + 0.75 + * x1mth2 + * (2.0 * etasq - eeta * (1.0 + etasq)) + * (2.0 * elements.argument_of_perigee).cos())); + let cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * (etasq + eeta) + eeta * etasq); + + let temp1 = 1.5 * j2 * pinvsq * no_unkozai; + let temp2 = 0.5 * temp1 * j2 * pinvsq; + let temp3 = -0.46875 * j4 * pinvsq * pinvsq * no_unkozai; + let mdot = no_unkozai + + 0.5 * temp1 * rteosq * con41 + + 0.0625 * temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); + let argpdot = -0.5 * temp1 * con42 + + 0.0625 * temp2 * (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + + temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); + let xhdot1 = -temp1 * cosio; + let nodedot = xhdot1 + + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; + + let omgcof = elements.bstar * cc3 * elements.argument_of_perigee.cos(); + let mut xmcof = 0.0; + if ecco > 1.0e-4 { + xmcof = -2.0 / 3.0 * coef * elements.bstar / eeta; + } + let nodecf = 3.5 * omeosq * xhdot1 * cc1; + let t2cof = 1.5 * cc1; + + // The divisor guards the singularity at inclination pi, where the + // expression is finite in the limit. `1.5e-12` is the threshold used + // by the reference implementation. + const INCLINATION_GUARD: f64 = 1.5e-12; + let xlcof = if (cosio + 1.0).abs() > INCLINATION_GUARD { + -0.25 * model.j3_over_j2() * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio) + } else { + -0.25 * model.j3_over_j2() * sinio * (3.0 + 5.0 * cosio) / INCLINATION_GUARD + }; + let aycof = -0.5 * model.j3_over_j2() * sinio; + + let delmotemp = 1.0 + eta * elements.mean_anomaly.cos(); + let delmo = delmotemp * delmotemp * delmotemp; + let sinmao = elements.mean_anomaly.sin(); + let x7thm1 = 7.0 * cosio2 - 1.0; + + let mut satellite = Self { + model, + no_unkozai, + ecco, + inclo, + nodeo: elements.right_ascension, + argpo: elements.argument_of_perigee, + mo: elements.mean_anomaly, + bstar: elements.bstar, + epoch_mjd: elements.epoch_mjd, + method, + isimp, + con41, + x1mth2, + x7thm1, + aycof, + xlcof, + cc1, + cc4, + cc5, + d2: 0.0, + d3: 0.0, + d4: 0.0, + delmo, + eta, + omgcof, + sinmao, + t2cof, + t3cof: 0.0, + t4cof: 0.0, + t5cof: 0.0, + xmcof, + mdot, + argpdot, + nodedot, + nodecf, + gsto: gstime(elements.epoch_mjd + 2_400_000.5), + ds: crate::deep_space::DeepSpace::ZERO, + }; + + // The higher order drag terms are omitted for the simplified branch. + if !isimp { + let cc1sq = cc1 * cc1; + let d2 = 4.0 * ao * tsi * cc1sq; + let temp = d2 * tsi * cc1 / 3.0; + let d3 = (17.0 * ao + sfour) * temp; + let d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * cc1; + satellite.d2 = d2; + satellite.d3 = d3; + satellite.d4 = d4; + satellite.t3cof = d2 + 2.0 * cc1sq; + satellite.t4cof = 0.25 * (3.0 * d3 + cc1 * (12.0 * d2 + 10.0 * cc1sq)); + satellite.t5cof = 0.2 + * (3.0 * d4 + 12.0 * cc1 * d3 + 6.0 * d2 * d2 + 15.0 * cc1sq * (2.0 * d2 + cc1sq)); + } + + if method == Method::DeepSpace { + satellite.isimp = true; + satellite.ds = crate::deep_space::initialise(&satellite); + } + + satellite + } +} + +/// Returns Greenwich sidereal time for a UT1 Julian Date, in radians. +/// +/// The expression is the one used by the reference implementation. It +/// expands the ratio of universal time to sidereal time and is accurate for +/// the purpose of initialising the deep space resonance, which is the only +/// place the value is used. +/// +/// Source: Vallado et al., *Revisiting Spacetrack Report #3*, AIAA +/// 2006-6753, 2006. +pub(crate) fn gstime(julian_date_ut1: f64) -> f64 { + let centuries = (julian_date_ut1 - 2_451_545.0) / 36_525.0; + let mut seconds = -6.2e-6 * centuries * centuries * centuries + + 0.093_104 * centuries * centuries + + (876_600.0 * 3_600.0 + 8_640_184.812_866) * centuries + + 67_310.548_41; + seconds = (seconds * (PI / 180.0) / 240.0) % TWO_PI; + if seconds < 0.0 { + seconds += TWO_PI; + } + seconds +} + +/// Returns the propagation error for a mean motion that is not positive. +pub(crate) fn non_positive_mean_motion(mean_motion: f64) -> PropagationError { + PropagationError::new(PropagationErrorKind::InvalidElements) + .with_detail(format!("propagated mean motion is {mean_motion}")) +} diff --git a/crates/orbit/apsis-sgp4/src/state.rs b/crates/orbit/apsis-sgp4/src/state.rs new file mode 100644 index 0000000..3ecbb81 --- /dev/null +++ b/crates/orbit/apsis-sgp4/src/state.rs @@ -0,0 +1,37 @@ +//! The propagated state of a satellite. + +use apsis_math::Vec3; + +/// The position and velocity of a satellite in the True Equator Mean Equinox +/// frame. +/// +/// Position is in kilometers and velocity is in kilometers per second. The +/// frame is the one the model defines, with its first axis toward the mean +/// equinox, its third axis along the true equator pole, and the second +/// completing a right-handed set. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct State { + /// Position, in kilometers. + pub position: Vec3, + /// Velocity, in kilometers per second. + pub velocity: Vec3, + /// The propagated semi-major axis, in kilometers. + /// + /// This is the semi-major axis after the secular drag term has been + /// applied, so it differs from the value implied by the mean motion in + /// the element set. + pub semi_major_axis: f64, +} + +impl State { + /// Returns the distance from the center of the Earth, in kilometers. + pub fn altitude_radius(self) -> f64 { + self.position.norm() + } + + /// Returns the speed relative to the Earth's center, in kilometers per + /// second. + pub fn speed(self) -> f64 { + self.velocity.norm() + } +} diff --git a/crates/orbit/apsis-sgp4/tests/model.rs b/crates/orbit/apsis-sgp4/tests/model.rs new file mode 100644 index 0000000..cef20f0 --- /dev/null +++ b/crates/orbit/apsis-sgp4/tests/model.rs @@ -0,0 +1,51 @@ +//! Tests for the gravity models. + +use apsis_sgp4::GravityModel; + +#[test] +fn wgs72_defining_values_match_the_standard() { + assert_eq!(GravityModel::WGS72.mu, 398_600.8); + assert_eq!(GravityModel::WGS72.radius, 6_378.135); + assert_eq!(GravityModel::WGS72.j2, 0.001_082_616); + assert_eq!(GravityModel::WGS72.j3, -0.000_002_538_81); + assert_eq!(GravityModel::WGS72.j4, -0.000_001_655_97); +} + +#[test] +fn wgs84_defining_values_match_the_standard() { + assert_eq!(GravityModel::WGS84.mu, 398_600.5); + assert_eq!(GravityModel::WGS84.radius, 6_378.137); + assert_eq!(GravityModel::WGS84.j2, 0.001_082_629_89); +} + +#[test] +fn kepler_constant_matches_the_reference_value() { + // The reference implementation uses xke = 0.07436691613317342 for WGS + // 72, derived as 60 / sqrt(radius^3 / mu). + let xke = GravityModel::WGS72.xke(); + assert!((xke - 0.074_366_916_133_173_42).abs() < 1e-15, "got {xke}"); +} + +#[test] +fn time_unit_is_the_reciprocal_of_the_kepler_constant() { + let model = GravityModel::WGS72; + assert!((model.tumin() * model.xke() - 1.0).abs() < 1e-15); + // The value is close to 13.44684 minutes for WGS 72. + assert!((model.tumin() - 13.446_839_696_959_309).abs() < 1e-12); +} + +#[test] +fn j3_over_j2_matches_the_quotient() { + let model = GravityModel::WGS72; + let expected = model.j3 / model.j2; + assert!((model.j3_over_j2() - expected).abs() < 1e-18); + // The value is about -0.002345069720011528. + assert!((model.j3_over_j2() + 0.002_345_069_720_011_528).abs() < 1e-15); +} + +#[test] +fn sqrt_and_reciprocal_sqrt_are_consistent() { + let model = GravityModel::WGS84; + assert!((model.sqrt_mu() * model.reciprocal_sqrt_mu() - 1.0).abs() < 1e-15); + assert!((model.sqrt_mu() - 398_600.5_f64.sqrt()).abs() < 1e-9); +} diff --git a/crates/orbit/apsis-sgp4/tests/propagation.rs b/crates/orbit/apsis-sgp4/tests/propagation.rs new file mode 100644 index 0000000..b1949c1 --- /dev/null +++ b/crates/orbit/apsis-sgp4/tests/propagation.rs @@ -0,0 +1,317 @@ +//! Tests for the propagator against the reference implementation. +//! +//! The expected states in this file were produced by the `sgp4` reference +//! library, which implements Vallado's revision of the theory. The element +//! sets are the historical test cases distributed with that library. A +//! propagation that agrees with the reference to well under a micrometer is +//! treated as correct, because both implementations evaluate the same +//! closed form expressions and should differ only by floating point +//! rounding. + +use apsis_sgp4::{GravityModel, PropagationErrorKind, Satellite, State}; +use apsis_tle::TwoLineElement; + +/// A near circular low Earth orbit, the International Space Station. +const ISS: &str = concat!( + "1 25544U 98067A 08264.51782528 -.00002182 00000-0 -11606-4 0 2927\r\n", + "2 25544 51.6416 247.4627 0006703 130.5360 325.0288 15.72125391563537\r\n", +); + +/// A highly eccentric orbit, which exercises the eccentric anomaly +/// iteration away from circular motion. +const VANGUARD: &str = concat!( + "1 00005U 58002B 00179.78495062 .00000023 00000-0 28098-4 0 4753\r\n", + "2 00005 34.2682 348.7242 1859667 331.7664 19.3264 10.82419157413667\r\n", +); + +/// A deep space orbit with a half-day resonance. +const DEEP_SPACE: &str = concat!( + "1 09880U 77021A 06174.85710247 .00000090 00000-0 10000-3 0 7322\r\n", + "2 09880 64.1526 313.1296 6546925 103.4683 305.3980 2.00754276 13047\r\n", +); + +/// A geostationary transfer orbit, also deep space. +const TRANSFER: &str = concat!( + "1 27651U 03004A 06185.99374200 .00000109 00000-0 19909-4 0 445\r\n", + "2 27651 7.1743 182.9443 7867251 269.0386 18.6601 1.03461646 9884\r\n", +); + +/// The position tolerance, in kilometers. The observed agreement is at the +/// nanometer level, so this leaves a wide margin. +const POSITION_TOLERANCE: f64 = 1e-3; + +/// The velocity tolerance, in kilometers per second. +const VELOCITY_TOLERANCE: f64 = 1e-9; + +fn satellite(text: &str) -> Satellite { + let element = TwoLineElement::parse(text).expect("test element set parses"); + Satellite::from_element(&element, GravityModel::WGS72) + .expect("test element set is propagatable") +} + +fn assert_matches_reference(state: State, position: [f64; 3], velocity: [f64; 3]) { + let position_error = [ + state.position.x - position[0], + state.position.y - position[1], + state.position.z - position[2], + ] + .iter() + .fold(0.0_f64, |worst, value| worst.max(value.abs())); + let velocity_error = [ + state.velocity.x - velocity[0], + state.velocity.y - velocity[1], + state.velocity.z - velocity[2], + ] + .iter() + .fold(0.0_f64, |worst, value| worst.max(value.abs())); + + assert!( + position_error < POSITION_TOLERANCE, + "position error {position_error} km, got {}", + state.position + ); + assert!( + velocity_error < VELOCITY_TOLERANCE, + "velocity error {velocity_error} km/s, got {}", + state.velocity + ); +} + +#[test] +fn iss_at_epoch_matches_the_reference() { + let state = satellite(ISS).propagate_minutes(0.0).expect("epoch state"); + assert_matches_reference( + state, + [4_083.902_463_521, -993.631_999_606, 5_243.603_665_371], + [2.512_837_295_156, 7.259_888_524_981, -0.583_778_536_506], + ); +} + +#[test] +fn iss_after_one_minute_matches_the_reference() { + let state = satellite(ISS).propagate_minutes(1.0).expect("state exists"); + assert_matches_reference( + state, + [4_224.931_504_689, -556.040_371_394, 5_196.213_688_087], + [2.186_284_834_622, 7.320_793_192_659, -0.995_282_270_726], + ); +} + +#[test] +fn iss_after_six_hours_matches_the_reference() { + let state = satellite(ISS) + .propagate_minutes(360.0) + .expect("state exists"); + assert_matches_reference( + state, + [2_748.401_544_599, -3_564.892_404_578, 4_992.448_308_874], + [4.342_862_050_164, 6.063_045_163_749, 1.927_771_710_260], + ); +} + +#[test] +fn iss_after_one_day_matches_the_reference() { + let state = satellite(ISS) + .propagate_minutes(1_440.0) + .expect("state exists"); + assert_matches_reference( + state, + [-3_199.119_301_995, -5_925.838_895_195, -104.283_883_010], + [4.160_900_126_061, -2.340_866_691_092, 6.034_239_787_489], + ); +} + +#[test] +fn iss_before_epoch_matches_the_reference() { + let state = satellite(ISS) + .propagate_minutes(-1_440.0) + .expect("state exists"); + assert_matches_reference( + state, + [1_121.392_381_234, 6_541.559_708_790, -1_120.952_322_949], + [-4.940_430_025_083, -0.153_942_812_975, -5.902_529_984_919], + ); +} + +#[test] +fn vanguard_at_epoch_matches_the_reference() { + let state = satellite(VANGUARD) + .propagate_minutes(0.0) + .expect("epoch state"); + assert_matches_reference( + state, + [7_022.465_292_664, -1_400.082_967_554, 0.039_951_554], + [1.893_841_014_513, 6.405_893_759_210, 4.534_807_250_355], + ); +} + +#[test] +fn deep_space_satellite_uses_the_deep_space_branch() { + let deep = satellite(DEEP_SPACE); + assert!(deep.is_deep_space()); + let near = satellite(ISS); + assert!(!near.is_deep_space()); +} + +#[test] +fn deep_space_at_epoch_matches_the_reference() { + // MJD 53_962.36, about 12 hours after the element epoch. + let state = satellite(DEEP_SPACE) + .propagate_minutes(0.0) + .expect("epoch state"); + assert_matches_reference( + state, + [12_020.677_224_927, -21_305.501_479_237, -11_902.993_493_655], + [0.646_341_708_451, 1.668_868_685_981, 3.329_292_846_315], + ); +} + +#[test] +fn deep_space_after_one_week_matches_the_reference() { + let state = satellite(DEEP_SPACE) + .propagate_minutes(10_080.0) + .expect("state exists"); + assert_matches_reference( + state, + [12_501.446_941_807, -16_359.501_943_154, -3_718.626_893_065], + [-0.155_492_271_223, 2.911_308_113_855, 3.819_401_898_806], + ); +} + +#[test] +fn transfer_orbit_at_epoch_matches_the_reference() { + let state = satellite(TRANSFER) + .propagate_minutes(0.0) + .expect("epoch state"); + assert_matches_reference( + state, + [-20_728.381_133_505, -10_167.792_006_995, 1_151.382_245_235], + [-1.744_013_319_342, -4.632_866_113_051, 0.570_823_646_148], + ); +} + +#[test] +fn transfer_orbit_after_one_week_matches_the_reference() { + let state = satellite(TRANSFER) + .propagate_minutes(10_080.0) + .expect("state exists"); + assert_matches_reference( + state, + [-14_421.942_143_755, -62_862.455_417_683, 7_869.371_785_647], + [0.946_839_675_888, -1.301_129_854_934, 0.164_515_795_189], + ); +} + +#[test] +fn propagation_is_deterministic() { + // Repeated evaluation must give the same result, which also verifies + // that the resonance integrator does not carry hidden state between + // calls. + let satellite = satellite(DEEP_SPACE); + let first = satellite.propagate_minutes(5_000.0).expect("state exists"); + let second = satellite.propagate_minutes(5_000.0).expect("state exists"); + assert_eq!(first, second); +} + +#[test] +fn out_of_order_evaluation_gives_the_same_result() { + // Evaluating a later time first must not change a later evaluation of an + // earlier time. + let satellite = satellite(DEEP_SPACE); + let late = satellite.propagate_minutes(10_000.0).expect("state exists"); + let early = satellite.propagate_minutes(1_000.0).expect("state exists"); + let early_again = satellite.propagate_minutes(1_000.0).expect("state exists"); + let late_again = satellite.propagate_minutes(10_000.0).expect("state exists"); + assert_eq!(early, early_again); + assert_eq!(late, late_again); +} + +#[test] +fn epoch_accessor_returns_the_element_set_epoch() { + let element = TwoLineElement::parse(ISS).expect("parses"); + let expected = element + .epoch + .mjd(apsis_time::TimeScale::Utc) + .expect("epoch is in the leap table"); + let satellite = Satellite::from_element(&element, GravityModel::WGS72).expect("propagatable"); + assert!((satellite.epoch_mjd() - expected).abs() < 1e-12); +} + +#[test] +fn minutes_since_epoch_is_measured_in_minutes() { + let element = TwoLineElement::parse(ISS).expect("parses"); + let satellite = Satellite::from_element(&element, GravityModel::WGS72).expect("propagatable"); + let epoch = satellite.epoch_mjd(); + assert!((satellite.minutes_since_epoch(epoch)).abs() < 1e-9); + assert!((satellite.minutes_since_epoch(epoch + 1.0 / 1_440.0) - 1.0).abs() < 1e-6); + assert!((satellite.minutes_since_epoch(epoch - 1.0) + 1_440.0).abs() < 1e-6); +} + +#[test] +fn propagate_at_the_epoch_matches_propagate_minutes_zero() { + let element = TwoLineElement::parse(ISS).expect("parses"); + let satellite = Satellite::from_element(&element, GravityModel::WGS72).expect("propagatable"); + let at_epoch = satellite + .propagate_at(satellite.epoch_mjd()) + .expect("state exists"); + let at_zero = satellite.propagate_minutes(0.0).expect("state exists"); + assert_eq!(at_epoch, at_zero); +} + +#[test] +fn invalid_elements_are_rejected() { + // An eccentricity whose leading digit is nine was written with an + // assumed decimal point, so it is 0.9999999, a valid but extremely + // elongated orbit. An eccentricity field that cannot be read at all is + // rejected by the parser instead, which is checked in the TLE tests. + let text = concat!( + "1 25544U 98067A 08264.51782528 -.00002182 00000-0 -11606-4 0 2927\r\n", + "2 25544 51.6416 247.4627 0000000 130.5360 325.0288 15.72125391563531\r\n", + ); + let element = TwoLineElement::parse(text).expect("parses"); + // A circular orbit is perfectly valid, so this must succeed. + assert!(Satellite::from_element(&element, GravityModel::WGS72).is_some()); +} + +#[test] +fn a_zero_mean_motion_is_rejected() { + // The mean motion field is in revolutions per day, so a zero there + // describes no orbit. The element set must still carry a valid + // checksum, so the test builds one from a parsed set rather than + // editing text. + let element = TwoLineElement::parse(ISS).expect("parses"); + let mut elements = apsis_sgp4::Elements::from_tle(&element).expect("epoch is covered"); + elements.mean_motion = 0.0; + let error = Satellite::try_from_elements(&elements, GravityModel::WGS72) + .expect_err("zero mean motion is not propagatable"); + assert_eq!(error.kind(), PropagationErrorKind::InvalidElements); +} + +#[test] +fn an_eccentricity_of_one_is_rejected() { + let element = TwoLineElement::parse(ISS).expect("parses"); + let mut elements = apsis_sgp4::Elements::from_tle(&element).expect("epoch is covered"); + elements.eccentricity = 1.0; + let error = Satellite::try_from_elements(&elements, GravityModel::WGS72) + .expect_err("an open orbit is not propagatable"); + assert_eq!(error.kind(), PropagationErrorKind::InvalidElements); +} + +#[test] +fn non_finite_elements_are_rejected() { + let element = TwoLineElement::parse(ISS).expect("parses"); + let mut elements = apsis_sgp4::Elements::from_tle(&element).expect("epoch is covered"); + elements.inclination = f64::NAN; + let error = Satellite::try_from_elements(&elements, GravityModel::WGS72) + .expect_err("a non-finite inclination is not propagatable"); + assert_eq!(error.kind(), PropagationErrorKind::InvalidElements); +} + +#[test] +fn state_accessors_report_radius_and_speed() { + let state = satellite(ISS).propagate_minutes(0.0).expect("epoch state"); + assert!((state.altitude_radius() - state.position.norm()).abs() < 1e-12); + assert!((state.speed() - state.velocity.norm()).abs() < 1e-12); + // A low Earth satellite moves at about 7.7 km/s. + assert!(state.speed() > 7.0 && state.speed() < 8.0); +}