ba1212361f
Commit the previously-untracked standard directories and catalog them in the
root README alongside proof:
- aac/ — communication cargo class: portable boards + a ten-level
offline-first cascade (Apache-2.0 code / CC-BY-4.0 spec).
- map/ — geospatial cargo class: range-requestable signed tilesets +
an F0–F4 fidelity cascade (Apache-2.0 code / CC-BY-4.0 spec).
- sandbox/— isolated, metered, attested execution across the heterogeneous
compute fabric (Apache-2.0 code / CC-BY-4.0 spec).
- jouleclaw-otel-authz, jouleclaw-trust — new jouleclaw reference crates.
Root README now lists aac, map, sandbox, and proof in the catalog, the tree,
and the contribution guide.
Co-Authored-By: Claude Opus 4.8 <[email protected]>
679 lines
23 KiB
Rust
679 lines
23 KiB
Rust
//! SGP4 orbital propagator for near-Earth satellites.
|
|
//!
|
|
//! Implements the Simplified General Perturbations 4 (SGP4) model from
|
|
//! Vallado et al., "Revisiting Spacetrack Report #3" (2006). This is the
|
|
//! standard propagator for NORAD Two-Line Element sets.
|
|
//!
|
|
//! For deep-space satellites (period >= 225 min), see [`crate::sdp4`].
|
|
|
|
use chrono::{DateTime, Utc};
|
|
use serde::{Deserialize, Serialize};
|
|
|
|
use crate::constants::*;
|
|
use crate::error::{OrbitalError, OrbitalResult};
|
|
use crate::julian::{datetime_to_jd, epoch_to_jd};
|
|
use crate::sdp4::DeepSpaceState;
|
|
use crate::tle::TleElements;
|
|
|
|
/// Position and velocity in the True Equator Mean Equinox (TEME) frame.
|
|
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
|
|
pub struct EciPosition {
|
|
/// X position (km).
|
|
pub x: f64,
|
|
/// Y position (km).
|
|
pub y: f64,
|
|
/// Z position (km).
|
|
pub z: f64,
|
|
/// X velocity (km/s).
|
|
pub vx: f64,
|
|
/// Y velocity (km/s).
|
|
pub vy: f64,
|
|
/// Z velocity (km/s).
|
|
pub vz: f64,
|
|
}
|
|
|
|
impl EciPosition {
|
|
/// Distance from Earth center (km).
|
|
pub fn radius(&self) -> f64 {
|
|
(self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
|
|
}
|
|
|
|
/// Speed (km/s).
|
|
pub fn speed(&self) -> f64 {
|
|
(self.vx * self.vx + self.vy * self.vy + self.vz * self.vz).sqrt()
|
|
}
|
|
}
|
|
|
|
/// Propagation method: near-Earth (SGP4) or deep-space (SDP4).
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
|
|
pub(crate) enum PropagationMethod {
|
|
Near,
|
|
Deep,
|
|
}
|
|
|
|
/// Initialized SGP4/SDP4 state, ready for propagation.
|
|
///
|
|
/// Created from a TLE via [`Sgp4State::initialize`]. All the expensive
|
|
/// precomputation happens once at initialization. Propagation to any
|
|
/// time is then a cheap arithmetic operation.
|
|
#[derive(Debug, Clone, Serialize, Deserialize)]
|
|
pub struct Sgp4State {
|
|
/// Original TLE elements.
|
|
pub tle: TleElements,
|
|
|
|
/// Near-earth or deep-space method.
|
|
pub(crate) method: PropagationMethod,
|
|
|
|
// ── Precomputed orbital elements (Kozai) ─────────────────
|
|
pub(crate) no_kozai: f64,
|
|
pub(crate) a0: f64,
|
|
pub(crate) ecco: f64,
|
|
pub(crate) inclo: f64,
|
|
pub(crate) nodeo: f64,
|
|
pub(crate) argpo: f64,
|
|
pub(crate) mo: f64,
|
|
pub(crate) bstar: f64,
|
|
|
|
// ── Secular rate coefficients ────────────────────────────
|
|
pub(crate) mdot: f64,
|
|
pub(crate) argpdot: f64,
|
|
pub(crate) nodedot: f64,
|
|
pub(crate) nodecf: f64,
|
|
|
|
// ── Near-earth drag coefficients ─────────────────────────
|
|
pub(crate) cc1: f64,
|
|
pub(crate) cc4: f64,
|
|
pub(crate) cc5: f64,
|
|
pub(crate) d2: f64,
|
|
pub(crate) d3: f64,
|
|
pub(crate) d4: f64,
|
|
pub(crate) eta: f64,
|
|
pub(crate) omgcof: f64,
|
|
pub(crate) sinmao: f64,
|
|
pub(crate) t2cof: f64,
|
|
pub(crate) t3cof: f64,
|
|
pub(crate) t4cof: f64,
|
|
pub(crate) t5cof: f64,
|
|
pub(crate) x1mth2: f64,
|
|
pub(crate) x7thm1: f64,
|
|
pub(crate) xlcof: f64,
|
|
pub(crate) xmcof: f64,
|
|
pub(crate) xnodcf: f64,
|
|
pub(crate) con41: f64,
|
|
|
|
// ── Perigee flags ────────────────────────────────────────
|
|
pub(crate) isimp: bool,
|
|
|
|
// ── Epoch JD for time calculation ────────────────────────
|
|
pub(crate) epoch_jd: f64,
|
|
|
|
// ── Deep-space state (only for SDP4) ─────────────────────
|
|
pub(crate) deep: Option<DeepSpaceState>,
|
|
}
|
|
|
|
impl Sgp4State {
|
|
/// Initialize the SGP4/SDP4 propagator from a TLE.
|
|
///
|
|
/// This performs the equivalent of `sgp4init` from the Vallado reference
|
|
/// implementation. All secular and periodic perturbation terms are
|
|
/// precomputed for fast propagation.
|
|
pub fn initialize(tle: &TleElements) -> OrbitalResult<Self> {
|
|
// Validate eccentricity
|
|
if tle.eccentricity < 0.0 || tle.eccentricity >= 1.0 {
|
|
return Err(OrbitalError::InvalidEccentricity(tle.eccentricity));
|
|
}
|
|
|
|
let ecco = tle.eccentricity;
|
|
let inclo = tle.inclination;
|
|
let nodeo = tle.raan;
|
|
let argpo = tle.arg_perigee;
|
|
let mo = tle.mean_anomaly;
|
|
let bstar = tle.bstar;
|
|
|
|
// Convert mean motion from rev/day to rad/min
|
|
let no_unkozai = tle.mean_motion * TWOPI / MINUTES_PER_DAY;
|
|
|
|
let epoch_jd = epoch_to_jd(tle.epoch_year, tle.epoch_day);
|
|
|
|
// ── Recover original mean motion (no_kozai) and semi-major axis ──
|
|
let cosio = inclo.cos();
|
|
let sinio = inclo.sin();
|
|
let cosio2 = cosio * cosio;
|
|
let x1mth2 = 1.0 - cosio2;
|
|
let x7thm1 = 7.0 * cosio2 - 1.0;
|
|
|
|
let _con41 = -con41_calc(cosio2);
|
|
|
|
// Intermediate values for Kozai correction
|
|
let a1 = (XKE / no_unkozai).powf(2.0 / 3.0);
|
|
let d1 = 0.75 * J2 * (3.0 * cosio2 - 1.0) / ((1.0 - ecco * ecco).sqrt().powi(3));
|
|
let del_ = d1 / (a1 * a1);
|
|
let adel = a1
|
|
* (1.0 - del_ * del_ - del_ * (1.0 / 3.0 + 134.0 * del_ * del_ / 81.0));
|
|
let del_ = d1 / (adel * adel);
|
|
let no_kozai = no_unkozai / (1.0 + del_);
|
|
|
|
let ao = (XKE / no_kozai).powf(2.0 / 3.0);
|
|
let po = ao * (1.0 - ecco * ecco);
|
|
|
|
// Perigee and apogee altitudes
|
|
let rp = ao * (1.0 - ecco);
|
|
if rp < 1.0 {
|
|
return Err(OrbitalError::SatelliteDecayed);
|
|
}
|
|
let perige = (rp - 1.0) * EARTH_RADIUS_KM;
|
|
|
|
// Determine near-earth vs deep-space
|
|
let period = TWOPI / no_kozai; // minutes
|
|
let method = if period >= DEEP_SPACE_THRESHOLD_MIN {
|
|
PropagationMethod::Deep
|
|
} else {
|
|
PropagationMethod::Near
|
|
};
|
|
|
|
// ── SGP4 atmospheric drag and gravitational coefficients ──
|
|
|
|
// s and qzms constants based on perigee altitude
|
|
let mut sfour = 78.0 / EARTH_RADIUS_KM + 1.0;
|
|
let mut qzms24 = ((120.0 - 78.0) / EARTH_RADIUS_KM).powi(4);
|
|
|
|
if perige < 156.0 {
|
|
sfour = if perige < 98.0 {
|
|
20.0 / EARTH_RADIUS_KM + 1.0
|
|
} else {
|
|
perige / EARTH_RADIUS_KM + 1.0
|
|
};
|
|
let qzms24temp = ((120.0 - (sfour - 1.0) * EARTH_RADIUS_KM) / EARTH_RADIUS_KM).powi(4);
|
|
qzms24 = qzms24temp;
|
|
}
|
|
|
|
let pinvsq = 1.0 / (po * po);
|
|
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_kozai
|
|
* (ao * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq))
|
|
+ 0.375 * J2 * tsi / psisq * (3.0 * (1.0 - 3.0 * cosio2) + 1.0)
|
|
* (8.0 + 3.0 * etasq * (8.0 + etasq)));
|
|
let cc1 = bstar * cc2;
|
|
let cc3 = if ecco > 1.0e-4 {
|
|
-2.0 * coef * tsi * J3OJ2 * no_kozai * sinio / ecco
|
|
} else {
|
|
0.0
|
|
};
|
|
let cc4 = 2.0
|
|
* no_kozai
|
|
* coef1
|
|
* ao
|
|
* po.recip()
|
|
* (eta * (2.0 + 0.5 * etasq)
|
|
+ ecco * (0.5 + 2.0 * etasq)
|
|
- J2 * tsi / (ao * psisq)
|
|
* (-3.0 * (1.0 - 3.0 * cosio2)
|
|
* (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta))
|
|
+ 0.75
|
|
* x1mth2
|
|
* (2.0 * etasq - eeta * (1.0 + etasq))
|
|
* (2.0 * argpo).cos()));
|
|
let cc5 = 2.0 * coef1 * ao * po.recip() * (1.0 + 2.75 * (etasq + eeta) + eeta * etasq);
|
|
|
|
let temp1 = 1.5 * J2 * pinvsq * no_kozai;
|
|
let temp2 = 0.5 * temp1 * J2 * pinvsq;
|
|
let temp3 = -0.46875 * J4 * pinvsq * pinvsq * no_kozai;
|
|
|
|
// Vallado's exact formulation for mdot
|
|
let beta_sq = 1.0 - ecco * ecco;
|
|
let beta = beta_sq.sqrt();
|
|
let mdot = no_kozai
|
|
+ 0.5 * temp1 * beta * (3.0 * cosio2 - 1.0) / beta_sq
|
|
+ 0.0625 * temp2 * beta
|
|
* (13.0 - 78.0 * cosio2 + 137.0 * cosio2 * cosio2)
|
|
/ (beta_sq * beta_sq);
|
|
|
|
let argpdot = -0.5 * temp1 * (1.0 - 5.0 * cosio2)
|
|
+ 0.0625 * temp2 * (7.0 - 114.0 * cosio2 + 395.0 * cosio2 * cosio2)
|
|
+ temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio2 * cosio2);
|
|
|
|
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 nodecf = 3.5 * po.recip().powi(2) * xhdot1 * cc1;
|
|
let xmcof = if ecco > 1.0e-4 {
|
|
-TWOPI / MINUTES_PER_DAY * coef / (3.0 * eeta) * bstar
|
|
} else {
|
|
0.0
|
|
};
|
|
|
|
let omgcof = bstar * cc3 * argpo.cos();
|
|
let sinmao = mo.sin();
|
|
let xlcof = if inclo.abs() > 1.0e-12 {
|
|
-0.25 * J3OJ2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio)
|
|
} else {
|
|
-0.25 * J3OJ2 * sinio * (3.0 + 5.0 * cosio) / (2.0 * 1.0e-12)
|
|
};
|
|
let xnodcf = nodecf;
|
|
|
|
let t2cof = 1.5 * cc1;
|
|
|
|
// Simplified flag: skip higher-order drag for high perigee or tiny ecc
|
|
let isimp = perige < 220.0;
|
|
|
|
let (d2, d3, d4, t3cof, t4cof, t5cof) = if !isimp {
|
|
let c1sq = cc1 * cc1;
|
|
let d2 = 4.0 * ao * tsi * c1sq;
|
|
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;
|
|
let t3cof = d2 + 2.0 * c1sq;
|
|
let t4cof = 0.25 * (3.0 * d3 + cc1 * (12.0 * d2 + 10.0 * c1sq));
|
|
let t5cof = 0.2
|
|
* (3.0 * d4 + 12.0 * cc1 * d3 + 6.0 * d2 * d2 + 15.0 * c1sq * (2.0 * d2 + c1sq));
|
|
(d2, d3, d4, t3cof, t4cof, t5cof)
|
|
} else {
|
|
(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
|
};
|
|
|
|
// ── Deep-space initialization ────────────────────────
|
|
let deep = if method == PropagationMethod::Deep {
|
|
Some(DeepSpaceState::initialize(
|
|
epoch_jd, ecco, inclo, nodeo, argpo, mo, no_kozai, bstar,
|
|
))
|
|
} else {
|
|
None
|
|
};
|
|
|
|
Ok(Sgp4State {
|
|
tle: tle.clone(),
|
|
method,
|
|
no_kozai,
|
|
a0: ao,
|
|
ecco,
|
|
inclo,
|
|
nodeo,
|
|
argpo,
|
|
mo,
|
|
bstar,
|
|
mdot,
|
|
argpdot,
|
|
nodedot,
|
|
nodecf,
|
|
cc1,
|
|
cc4,
|
|
cc5,
|
|
d2,
|
|
d3,
|
|
d4,
|
|
eta,
|
|
omgcof,
|
|
sinmao,
|
|
t2cof,
|
|
t3cof,
|
|
t4cof,
|
|
t5cof,
|
|
x1mth2,
|
|
x7thm1,
|
|
xlcof,
|
|
xmcof,
|
|
xnodcf,
|
|
con41: -con41_calc(cosio2),
|
|
isimp,
|
|
epoch_jd,
|
|
deep,
|
|
})
|
|
}
|
|
|
|
/// Propagate to a given number of minutes since TLE epoch.
|
|
///
|
|
/// Returns the satellite position and velocity in the TEME
|
|
/// (True Equator Mean Equinox) reference frame.
|
|
pub fn propagate(&self, tsince: f64) -> OrbitalResult<EciPosition> {
|
|
|
|
// ── Secular effects of atmospheric drag and gravitation ──
|
|
let xmdf = self.mo + self.mdot * tsince;
|
|
let argpdf = self.argpo + self.argpdot * tsince;
|
|
let nodedf = self.nodeo + self.nodedot * tsince;
|
|
|
|
let mut mp = xmdf;
|
|
let tsq = tsince * tsince;
|
|
let node = nodedf + self.xnodcf * tsq;
|
|
let argp;
|
|
let mut tempa = 1.0 - self.cc1 * tsince;
|
|
let mut tempe = self.bstar * self.cc4 * tsince;
|
|
let mut templ = self.t2cof * tsq;
|
|
|
|
if !self.isimp {
|
|
let delomg = self.omgcof * tsince;
|
|
let dmp = if self.ecco > 1.0e-4 {
|
|
self.xmcof
|
|
* ((1.0 + self.eta * xmdf.cos()).powi(3) - (1.0 + self.eta * self.mo.cos()).powi(3))
|
|
} else {
|
|
0.0
|
|
};
|
|
mp = xmdf + delomg + dmp;
|
|
argp = argpdf - delomg - dmp;
|
|
|
|
let tcube = tsq * tsince;
|
|
let tfour = tsince * tcube;
|
|
tempa = tempa - self.d2 * tsq - self.d3 * tcube - self.d4 * tfour;
|
|
tempe += self.bstar * self.cc5 * (mp.sin() - self.sinmao);
|
|
templ = templ + self.t3cof * tcube + self.t4cof * tfour + self.t5cof * tfour * tsince;
|
|
} else {
|
|
argp = argpdf;
|
|
}
|
|
|
|
// Apply deep-space secular effects if SDP4
|
|
let (mp, node, argp, eccentricity, inclination, no_kozai_adj) = if self.method == PropagationMethod::Deep {
|
|
if let Some(ref deep) = self.deep {
|
|
let (mp2, node2, argp2, ecc2, inc2, nm2) =
|
|
deep.secular(tsince, mp, node, argp, self.ecco, self.inclo, self.no_kozai);
|
|
(mp2, node2, argp2, ecc2, inc2, nm2)
|
|
} else {
|
|
(mp, node, argp, self.ecco, self.inclo, self.no_kozai)
|
|
}
|
|
} else {
|
|
(mp, node, argp, self.ecco, self.inclo, self.no_kozai)
|
|
};
|
|
|
|
let mut nm = no_kozai_adj;
|
|
let mut em = eccentricity;
|
|
let mut xinc = inclination;
|
|
|
|
if self.method == PropagationMethod::Near {
|
|
nm = self.no_kozai;
|
|
em = self.ecco;
|
|
xinc = self.inclo;
|
|
}
|
|
|
|
if tempa <= 0.0 {
|
|
return Err(OrbitalError::SatelliteDecayed);
|
|
}
|
|
|
|
if self.method == PropagationMethod::Near {
|
|
nm = self.no_kozai;
|
|
}
|
|
|
|
let am = (XKE / (tempa * (if self.method == PropagationMethod::Near { self.no_kozai } else { nm }))).powf(2.0 / 3.0) * tempa * tempa;
|
|
if self.method == PropagationMethod::Near {
|
|
nm = XKE / am.powf(1.5);
|
|
}
|
|
if self.method == PropagationMethod::Near {
|
|
em = self.ecco - tempe;
|
|
}
|
|
|
|
// Fix eccentricity
|
|
if !(-0.001..1.0).contains(&em) {
|
|
return Err(OrbitalError::InvalidEccentricity(em));
|
|
}
|
|
if em < 1.0e-6 {
|
|
em = 1.0e-6;
|
|
}
|
|
|
|
let mut mp = mp;
|
|
if self.method == PropagationMethod::Near {
|
|
mp += self.no_kozai * templ;
|
|
}
|
|
|
|
let _xlm = mp + argp + node;
|
|
|
|
let mut ep = em;
|
|
let mut xincp = xinc;
|
|
let mut argpp = argp;
|
|
let mut nodep = node;
|
|
|
|
// Deep-space periodic effects
|
|
if self.method == PropagationMethod::Deep {
|
|
if let Some(ref deep) = self.deep {
|
|
let (ep2, xincp2, argpp2, nodep2, mp2) =
|
|
deep.periodics(tsince, ep, xincp, argpp, nodep, mp);
|
|
ep = ep2;
|
|
xincp = xincp2;
|
|
argpp = argpp2;
|
|
nodep = nodep2;
|
|
mp = mp2;
|
|
}
|
|
}
|
|
|
|
// ── Long-period periodic terms ──────────────────────
|
|
let sinip = xincp.sin();
|
|
let cosip = xincp.cos();
|
|
|
|
let axnl = ep * argpp.cos();
|
|
let aynl = ep * argpp.sin() + if xincp.abs() > 1.0e-12 {
|
|
-0.5 * J3OJ2 * sinip / (1.0 + cosip)
|
|
} else {
|
|
-0.5 * J3OJ2 * sinip / (2.0 * 1.0e-12)
|
|
};
|
|
|
|
let xl = mp + argpp + nodep
|
|
+ (-0.25 * J3OJ2 * sinip * (3.0 + 5.0 * cosip) / (if (1.0 + cosip).abs() > 1.0e-12 { 1.0 + cosip } else { 2.0 * 1.0e-12 }))
|
|
* axnl;
|
|
|
|
// ── Solve Kepler's equation ─────────────────────────
|
|
let u = (xl - nodep).rem_euclid(TWOPI);
|
|
let mut eo1 = u;
|
|
let mut ktr: u32 = 0;
|
|
let mut sineo1;
|
|
let mut coseo1;
|
|
|
|
loop {
|
|
sineo1 = eo1.sin();
|
|
coseo1 = eo1.cos();
|
|
let tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl;
|
|
if tem5.abs() < SMALL {
|
|
break;
|
|
}
|
|
let tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
|
|
let delta = tem5.clamp(-0.95, 0.95);
|
|
eo1 += delta;
|
|
ktr += 1;
|
|
if ktr >= MAX_KEPLER_ITERATIONS || delta.abs() < 1.0e-12 {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// ── Short-period periodic terms ─────────────────────
|
|
let ecose = axnl * coseo1 + aynl * sineo1;
|
|
let esine = axnl * sineo1 - aynl * coseo1;
|
|
let el2 = axnl * axnl + aynl * aynl;
|
|
|
|
let pl = am * (1.0 - el2);
|
|
if pl < 0.0 {
|
|
return Err(OrbitalError::PropagationError(
|
|
"semi-latus rectum is negative".into(),
|
|
));
|
|
}
|
|
|
|
let rl = am * (1.0 - ecose);
|
|
let rdotl = am.sqrt() * esine / rl;
|
|
let rvdotl = pl.sqrt() / rl;
|
|
let betal = (1.0 - el2).sqrt();
|
|
|
|
let sinu = am / rl * (sineo1 - aynl - axnl * esine / (1.0 + betal));
|
|
let cosu = am / rl * (coseo1 - axnl + aynl * esine / (1.0 + betal));
|
|
|
|
let su = sinu.atan2(cosu);
|
|
let sin2u = (cosu + cosu) * sinu;
|
|
let cos2u = 1.0 - 2.0 * sinu * sinu;
|
|
let temp1 = 0.5 * J2 / pl;
|
|
let temp2 = temp1 / pl;
|
|
|
|
// SGP4 short-period corrections
|
|
let cosip2 = cosip * cosip;
|
|
let mrt = rl * (1.0 - 1.5 * temp2 * betal * (3.0 * cosip2 - 1.0))
|
|
+ 0.5 * temp1 * (1.0 - cosip2) * cos2u;
|
|
let su_corr = su
|
|
- 0.25 * temp2 * (7.0 * cosip2 - 1.0) * sin2u;
|
|
let xnode = nodep + 1.5 * temp2 * cosip * sin2u;
|
|
let xinc_corr = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
|
|
let rdot = rdotl - nm * temp1 * (1.0 - cosip2) * sin2u;
|
|
let rvdot = rvdotl + nm * temp1 * ((1.0 - cosip2) * cos2u + 1.5 * (1.0 - 3.0 * cosip2));
|
|
|
|
if mrt < 1.0 {
|
|
return Err(OrbitalError::SatelliteDecayed);
|
|
}
|
|
|
|
// ── Convert to position/velocity in TEME frame ──────
|
|
let sinsu = su_corr.sin();
|
|
let cossu = su_corr.cos();
|
|
let snod = xnode.sin();
|
|
let cnod = xnode.cos();
|
|
let sini = xinc_corr.sin();
|
|
let cosi = xinc_corr.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;
|
|
|
|
// Position in km, velocity in km/s
|
|
let r_km = mrt * EARTH_RADIUS_KM;
|
|
let rdot_kmps = rdot * VKMPERSEC;
|
|
let rvdot_kmps = rvdot * VKMPERSEC;
|
|
|
|
Ok(EciPosition {
|
|
x: r_km * ux,
|
|
y: r_km * uy,
|
|
z: r_km * uz,
|
|
vx: rdot_kmps * ux + rvdot_kmps * vx,
|
|
vy: rdot_kmps * uy + rvdot_kmps * vy,
|
|
vz: rdot_kmps * uz + rvdot_kmps * vz,
|
|
})
|
|
}
|
|
|
|
/// Propagate to a specific `DateTime<Utc>`.
|
|
pub fn propagate_to(&self, datetime: &DateTime<Utc>) -> OrbitalResult<EciPosition> {
|
|
let jd = datetime_to_jd(datetime);
|
|
let minutes_since_epoch = (jd - self.epoch_jd) * MINUTES_PER_DAY;
|
|
self.propagate(minutes_since_epoch)
|
|
}
|
|
|
|
/// Orbital period in minutes.
|
|
pub fn orbital_period_minutes(&self) -> f64 {
|
|
TWOPI / self.no_kozai
|
|
}
|
|
|
|
/// Orbital period in seconds.
|
|
pub fn orbital_period_seconds(&self) -> f64 {
|
|
self.orbital_period_minutes() * 60.0
|
|
}
|
|
}
|
|
|
|
/// Compute con41 = 3*cos^2(i) - 1 (negated because of how it's used in the propagator).
|
|
fn con41_calc(cosio2: f64) -> f64 {
|
|
3.0 * cosio2 - 1.0
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
use crate::tle::parse_tle;
|
|
|
|
const ISS_NAME: &str = "ISS (ZARYA)";
|
|
const ISS_LINE1: &str =
|
|
"1 25544U 98067A 24001.50000000 .00016717 00000-0 10270-3 0 9009";
|
|
const ISS_LINE2: &str =
|
|
"2 25544 51.6416 247.4627 0006703 30.0000 10.0000 15.49560000000007";
|
|
|
|
#[test]
|
|
fn test_initialize_iss() {
|
|
let tle = parse_tle(ISS_NAME, ISS_LINE1, ISS_LINE2).unwrap();
|
|
let state = Sgp4State::initialize(&tle).unwrap();
|
|
assert_eq!(state.method, PropagationMethod::Near);
|
|
assert!(state.no_kozai > 0.0);
|
|
assert!(state.orbital_period_minutes() < 225.0);
|
|
assert!((state.orbital_period_minutes() - 92.9).abs() < 1.0);
|
|
}
|
|
|
|
#[test]
|
|
fn test_propagate_epoch() {
|
|
let tle = parse_tle(ISS_NAME, ISS_LINE1, ISS_LINE2).unwrap();
|
|
let state = Sgp4State::initialize(&tle).unwrap();
|
|
let pos = state.propagate(0.0).unwrap();
|
|
|
|
// At epoch, ISS should be at roughly LEO altitude (400 km)
|
|
let alt_km = pos.radius() - EARTH_RADIUS_KM;
|
|
assert!(
|
|
alt_km > 300.0 && alt_km < 500.0,
|
|
"ISS altitude at epoch: {alt_km:.1} km"
|
|
);
|
|
|
|
// Velocity should be roughly 7.7 km/s for LEO
|
|
let speed = pos.speed();
|
|
assert!(
|
|
speed > 7.0 && speed < 8.5,
|
|
"ISS speed at epoch: {speed:.2} km/s"
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn test_propagate_multiple_times() {
|
|
let tle = parse_tle(ISS_NAME, ISS_LINE1, ISS_LINE2).unwrap();
|
|
let state = Sgp4State::initialize(&tle).unwrap();
|
|
|
|
// Propagate at multiple times and verify orbit stays reasonable
|
|
for minutes in [0.0, 10.0, 30.0, 60.0, 120.0, 360.0, 720.0, 1440.0] {
|
|
let pos = state.propagate(minutes).unwrap();
|
|
let alt_km = pos.radius() - EARTH_RADIUS_KM;
|
|
let speed = pos.speed();
|
|
assert!(
|
|
alt_km > 200.0 && alt_km < 600.0,
|
|
"ISS altitude at t={minutes}min: {alt_km:.1} km"
|
|
);
|
|
assert!(
|
|
speed > 7.0 && speed < 8.5,
|
|
"ISS speed at t={minutes}min: {speed:.2} km/s"
|
|
);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_invalid_eccentricity() {
|
|
let mut tle = parse_tle(ISS_NAME, ISS_LINE1, ISS_LINE2).unwrap();
|
|
tle.eccentricity = 1.5;
|
|
let result = Sgp4State::initialize(&tle);
|
|
assert!(matches!(result, Err(OrbitalError::InvalidEccentricity(_))));
|
|
}
|
|
|
|
#[test]
|
|
fn test_propagate_to_datetime() {
|
|
let tle = parse_tle(ISS_NAME, ISS_LINE1, ISS_LINE2).unwrap();
|
|
let state = Sgp4State::initialize(&tle).unwrap();
|
|
|
|
// Propagate to epoch should give same result as propagate(0)
|
|
let pos_epoch = state.propagate(0.0).unwrap();
|
|
let pos_dt = state.propagate_to(&tle.epoch).unwrap();
|
|
|
|
assert!((pos_epoch.x - pos_dt.x).abs() < 0.001);
|
|
assert!((pos_epoch.y - pos_dt.y).abs() < 0.001);
|
|
assert!((pos_epoch.z - pos_dt.z).abs() < 0.001);
|
|
}
|
|
|
|
#[test]
|
|
fn test_orbital_period() {
|
|
let tle = parse_tle(ISS_NAME, ISS_LINE1, ISS_LINE2).unwrap();
|
|
let state = Sgp4State::initialize(&tle).unwrap();
|
|
|
|
// ISS mean motion is ~15.5 rev/day -> period ~92.9 min
|
|
let period = state.orbital_period_minutes();
|
|
let expected = MINUTES_PER_DAY / tle.mean_motion;
|
|
assert!(
|
|
(period - expected).abs() < 1.0,
|
|
"Period: {period:.2} min, expected ~{expected:.2} min"
|
|
);
|
|
}
|
|
}
|