From 78bc4d6f56b261ef2ed95bf4843df9c75fd77815 Mon Sep 17 00:00:00 2001 From: Evan Montgomery-Recht Date: Thu, 16 Jul 2026 22:41:47 -0400 Subject: [PATCH 1/2] feat(filter): equinoctial GVE + element-space orbital filtering (element-space-orbital-filtering) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Completes the element-space-orbital-filtering change (14/14 tasks) — the advanced-methods survey's space-accuracy centerpiece. All additive: existing models, trackers, and every calibrated benchmark unchanged. - thresh-core/orbital/gve: Gauss variational equations for the stored direct equinoctial set (a, h, k, p, q, lambda), transcribed from the fetched Stacey & D'Amico arXiv:2105.06516 Appendix A (naming map documented; the appendix's corrected Roth H-bar sign); perturbing acceleration as a time-aware inertial closure rotated into RSW internally (J2 enters as j2_acceleration alone; two-body secular analytic); sub-stepped RK4; near-singular (e,i ~ 1e-8) finite; cross-formulation equivalence vs the Cartesian two-body+J2 path in both directions across LEO/MEO/eccentric-through-perigee at measured sub-decimetre tolerances — the transcription validated by an independent in-tree formulation sharing only the force closure - thresh-filter/models/equinoctial: MotionModel over the 6D element state (KeplerJ2 conventions); mean longitude UNWRAPPED in filter state space with wrap-straddle and hundreds-of-radians proofs; elements->ECI position measurement mapping bitwise-equal to the existing conversion; process noise = isotropic acceleration PSD through the GVE input matrix (Stacey & D'Amico Eq. 5) — the same physical Q the Cartesian model encodes, so demonstration fairness is by construction - The measured payoff (thresh-eval/tests/orbital_coast_gap.rs, seeded, 50 runs, bitwise-deterministic): coasting a 7000 km LEO, the Cartesian EKF's position ANEES exits the 95% band once per revolution as the banana rotates (4.757 at 2.39 rev vs band [2.360, 3.716]; again 4.370 at 3.35 rev) while the equinoctial UKF stays inside at every one of nine swept gaps (2.58-2.91) — the element-space consistency claim, measured. Demonstration isolates coast-phase propagation via a shared warmup posterior (divergence + rationale recorded in design.md) - Benchmark invariance: all four calibrated scenarios digit-for-digit identical to the pre-change baseline Adversarially reviewed (4 lenses incl. a term-by-term re-fetch of the GVE source): 6 findings, 2 confirmed and fixed (the fetched paper is by Stacey & D'Amico, not "Sullivan" — the exact filled-from-memory citation fingerprint the provenance rule exists to catch; a 21x minute-conversion slip in design prose), 4 refuted. Co-Authored-By: Claude Fable 5 --- crates/thresh-core/src/orbital/gve.rs | 756 ++++++++++++++++++ crates/thresh-core/src/orbital/mod.rs | 1 + crates/thresh-eval/tests/orbital_coast_gap.rs | 463 +++++++++++ crates/thresh-filter/src/models.rs | 1 + .../thresh-filter/src/models/equinoctial.rs | 525 ++++++++++++ .../element-space-orbital-filtering/design.md | 13 +- .../element-space-orbital-filtering/tasks.md | 28 +- 7 files changed, 1770 insertions(+), 17 deletions(-) create mode 100644 crates/thresh-core/src/orbital/gve.rs create mode 100644 crates/thresh-eval/tests/orbital_coast_gap.rs create mode 100644 crates/thresh-filter/src/models/equinoctial.rs diff --git a/crates/thresh-core/src/orbital/gve.rs b/crates/thresh-core/src/orbital/gve.rs new file mode 100644 index 0000000..98caf8c --- /dev/null +++ b/crates/thresh-core/src/orbital/gve.rs @@ -0,0 +1,756 @@ +//! Gauss variational equations (GVE) for the stored direct/prograde +//! equinoctial element set `(a, h, k, p, q, λ)`. +//! +//! This module supplies the element-space dynamics counterpart to the +//! Cartesian [`crate::orbital::integrate`] tier: the per-element rate law +//! driven by a *perturbing* acceleration, plus a sub-stepped fixed-step RK4 +//! loop over those rates (mirroring `integrate.rs`'s role for the second-order +//! Cartesian system). The stored set is nonsingular at zero eccentricity and +//! zero inclination; the retrograde geometry `i = π` is out of scope +//! (`tan(i/2)` — hence `p`, `q` — diverges there). +//! +//! # Source of the equations (provenance — never from memory) +//! +//! The element-rate equations are transcribed from **Appendix A of** N. Stacey +//! & S. D'Amico, *"Analytical Process Noise Covariance Modeling for Absolute and +//! Relative Orbits"* (arXiv:2105.06516, Acta Astronautica; PDF fetched +//! 2026-07-16 from ), which states the Gauss +//! variational equations "formulated in equinoctial elements" for exactly this +//! set (their Eq. 42 defines `xE = (a, f, g, h, k, λ)`; Eq. 44–45 give +//! `dxE/dt = G(xE)·d + [0,…,0,n]ᵀ`; Eq. A.1 gives the scalar entries of `G`). +//! That appendix cites R. H. Battin, *An Introduction to the Mathematics and +//! Methods of Astrodynamics* (ref. 32) and E. A. Roth, *"The Gaussian form of +//! the variation-of-parameter equations formulated in equinoctial elements"* +//! (ref. 33), and explicitly notes that **Roth has a sign error on H̄** — the +//! form transcribed here is the appendix's corrected one, not Roth's. +//! +//! # Naming map: fetched source ⟷ this crate's stored set +//! +//! The fetched source names the eccentricity/inclination components `(f, g, h, +//! k)`; [`crate::orbital::OrbitalElements::Equinoctial`] stores `(h, k, p, q)`. +//! They are the same six numbers under this exact renaming: +//! +//! | fetched source | this crate's stored field | +//! |-------------------------------|---------------------------| +//! | `f = e·cos(ω+Ω)` | `k` | +//! | `g = e·sin(ω+Ω)` | `h` | +//! | `h = tan(i/2)·cosΩ` | `q` | +//! | `k = tan(i/2)·sinΩ` | `p` | +//! | `a`, `λ = M+ω+Ω` | `a`, `mean_longitude` | +//! +//! Every equation below is written in **this crate's stored naming** with the +//! substitution already applied, and each helper's doc quotes the source form +//! it came from so the transcription is auditable line by line. +//! +//! # Perturbing-acceleration seam (design Decision 2) +//! +//! The GVE consume the *perturbing* acceleration only — the two-body term is +//! the analytic secular rate `dλ/dt ⊇ n(a) = √(µ/a³)`, never part of the +//! perturbation input. The evaluator takes a time-aware inertial closure +//! `Fn(t, &r, &v) -> a_pert` and rotates it into the RSW frame internally, so +//! any force the force stack can express can later drive the GVE. For this +//! change the perturbation is J2 alone; [`j2_perturbation_acceleration`] +//! builds it (see its note on the historical `j2_acceleration` composite). + +use nalgebra::Vector3; + +use crate::orbital::gravity::{GravityModel, j2_acceleration, two_body_acceleration}; +use crate::orbital::state::{Frame, OrbitalElements, OrbitalState, keplerian_to_cartesian}; +use crate::time::Epoch; + +/// Arbitrary fixed Julian date (J2000.0) for the throwaway [`OrbitalState`] +/// built purely to reuse the crate's tested element conversions. The +/// equinoctial↔Keplerian↔Cartesian conversions are pure geometry — they read +/// neither the epoch nor the frame tag — so this value never affects any rate. +const CONVERSION_EPOCH_JD: f64 = 2_451_545.0; + +// --------------------------------------------------------------------------- +// RSW (radial / transverse / normal) rotation +// --------------------------------------------------------------------------- + +/// Right-handed RSW orbital basis built from an inertial position/velocity +/// pair: `R̂ = r̂` (radial), `Ŵ = (r×v)/‖r×v‖` (normal, along the +/// angular-momentum vector), `Ŝ = Ŵ × R̂` (transverse, completing the triad +/// toward the velocity). +/// +/// This is the RTN frame of the fetched source (its Eq. B.2 rotation +/// `R_{I→R} = [r̂ᵀ; (n̂×r̂)ᵀ; n̂ᵀ]` with `n̂ = (r×v)/‖r×v‖`). +/// +/// Domain: undefined only for the degenerate `‖r‖ = 0` or `‖r×v‖ = 0` +/// (rectilinear) states, which no bound orbit produces. +#[derive(Debug, Clone, Copy)] +pub struct RswBasis { + /// Radial unit vector `R̂ = r̂`. + pub radial: Vector3, + /// Transverse unit vector `Ŝ = Ŵ × R̂`. + pub transverse: Vector3, + /// Normal unit vector `Ŵ = (r×v)/‖r×v‖`. + pub normal: Vector3, +} + +impl RswBasis { + /// Construct the RSW basis from inertial position and velocity. + pub fn from_state(r: &Vector3, v: &Vector3) -> Self { + let radial = r.normalize(); + let normal = r.cross(v).normalize(); + let transverse = normal.cross(&radial); + Self { + radial, + transverse, + normal, + } + } + + /// Project an inertial acceleration onto the basis, returning the + /// `(radial, transverse, normal)` = `(d_r, d_t, d_n)` components the GVE + /// consume. + pub fn project(&self, accel: &Vector3) -> (f64, f64, f64) { + ( + accel.dot(&self.radial), + accel.dot(&self.transverse), + accel.dot(&self.normal), + ) + } +} + +// --------------------------------------------------------------------------- +// Perturbation seam +// --------------------------------------------------------------------------- + +/// The **J2 perturbation acceleration alone** (the oblateness term, two-body +/// removed) — the perturbing acceleration the GVE consume for this change. +/// +/// Note the seam: [`j2_acceleration`] returns two-body **plus** J2 combined +/// (documented historical behaviour in [`crate::orbital::gravity`], unlike +/// `j3_acceleration`/`j4_acceleration` which return their term alone), so the +/// pure J2 perturbation is that minus [`two_body_acceleration`]. This is the +/// same subtraction the gravity module's own tests use to isolate the +/// perturbation. Two-body must **not** appear here — it is the analytic +/// `dλ/dt ⊇ √(µ/a³)` secular term inside the GVE. +pub fn j2_perturbation_acceleration(pos: &Vector3, gravity: &GravityModel) -> Vector3 { + j2_acceleration(pos, gravity) - two_body_acceleration(pos, gravity) +} + +// --------------------------------------------------------------------------- +// Auxiliary scalars for one rate evaluation +// --------------------------------------------------------------------------- + +/// Everything a single element-rate evaluation needs at the current state: +/// the stored elements, the current inertial `(r, v)`, the true longitude's +/// sine/cosine, and the derived scalars shared across the coefficients. +/// +/// Grouped into a struct rather than plumbed as arguments to keep each rate +/// helper's signature small (avoiding `clippy::too_many_arguments`) while +/// letting the top-level evaluator stay a flat sequence of phase calls. +struct GveAux { + /// Semi-major axis `a` (m). + a: f64, + /// Stored `h = e·sin(ω+Ω)` (fetched source's `g`). + h: f64, + /// Stored `k = e·cos(ω+Ω)` (fetched source's `f`). + k: f64, + /// Stored `p = tan(i/2)·sinΩ` (fetched source's `k`). + p: f64, + /// Stored `q = tan(i/2)·cosΩ` (fetched source's `h`). + q: f64, + /// `sin l`, `cos l` of the true longitude `l = ν + ω + Ω`. + sin_l: f64, + cos_l: f64, + /// `W = 1 + k·cos l + h·sin l = 1 + e·cos ν` (`= p_slr/r`). Positive for + /// every bound orbit (`e < 1`); the singular denominator in most + /// coefficients. Vanishes only at apoapsis of a parabola (`e = 1`). + w: f64, + /// `η = √(1 − e²)`, in `(0, 1]`; `1 + η ∈ [1, 2]` is never zero. + eta: f64, + /// Specific angular momentum `L = √(µ·p_slr) = ‖r×v‖`. Vanishes only as + /// `e → 1` (`p_slr → 0`); the leading denominator of `Ā`, `B̄`, `K̄`. + ang_mom: f64, + /// Mean motion `n = √(µ/a³)`. + n: f64, + /// `√(p_slr/µ)`, the common leading factor of the eccentricity/inclination + /// coefficients. + sqrt_p_mu: f64, + /// Gravitational parameter `µ` (m³/s²). + mu: f64, + /// Inertial position (m) at the current elements. + r: Vector3, + /// Inertial velocity (m/s) at the current elements. + v: Vector3, +} + +impl GveAux { + /// Build the auxiliaries from the stored elements `[a, h, k, p, q, λ]`. + /// + /// The true longitude and `(r, v)` come from the crate's tested element + /// conversions (equinoctial → Keplerian → Cartesian); `e`, `η`, `p_slr`, + /// `L`, `n` come straight from the stored `a, h, k` (exact, no conversion). + fn from_elements(elements: &[f64; 6], mu: f64) -> Self { + let [a, h, k, p, q, mean_longitude] = *elements; + let state = OrbitalState { + elements: OrbitalElements::Equinoctial { + sma: a, + h, + k, + p, + q, + mean_longitude, + }, + mu, + frame: Frame::Teme, + epoch: Epoch::from_jde_utc(CONVERSION_EPOCH_JD), + }; + // Equinoctial → Keplerian is infallible (never the `Tle` variant), so + // the true longitude l = ν + ω + Ω is always available. + let (sma, ecc, inc, raan, argp, true_anomaly) = state + .as_keplerian() + .expect("equinoctial elements always convert to Keplerian"); + let true_longitude = true_anomaly + argp + raan; + let (r, v) = keplerian_to_cartesian(sma, ecc, inc, raan, argp, true_anomaly, mu); + + let (sin_l, cos_l) = true_longitude.sin_cos(); + let ecc2 = h * h + k * k; + let eta = (1.0 - ecc2).sqrt(); + let semi_latus_rectum = a * (1.0 - ecc2); + let ang_mom = (mu * semi_latus_rectum).sqrt(); + let n = (mu / (a * a * a)).sqrt(); + let sqrt_p_mu = (semi_latus_rectum / mu).sqrt(); + let w = 1.0 + k * cos_l + h * sin_l; + + Self { + a, + h, + k, + p, + q, + sin_l, + cos_l, + w, + eta, + ang_mom, + n, + sqrt_p_mu, + mu, + r, + v, + } + } + + /// `χ = p·cos l − q·sin l` — the fetched source's `(k·cos l − h·sin l)` + /// cross-track factor, shared by `Ē`, `H̄`, and `M̄`. + fn chi(&self) -> f64 { + self.p * self.cos_l - self.q * self.sin_l + } +} + +// --------------------------------------------------------------------------- +// Per-element rate helpers (one phase each; fetched-source coefficient quoted) +// --------------------------------------------------------------------------- + +/// `da/dt = Ā·d_r + B̄·d_t` with (source Eq. A.1, renamed `f→k, g→h`): +/// `Ā = (2a²/L)(k·sin l − h·cos l)`, `B̄ = 2a²·W/L`. +fn rate_a(aux: &GveAux, d_r: f64, d_t: f64) -> f64 { + let two_a2_over_l = 2.0 * aux.a * aux.a / aux.ang_mom; + let psi = aux.k * aux.sin_l - aux.h * aux.cos_l; // source (f sin l − g cos l) + two_a2_over_l * (psi * d_r + aux.w * d_t) +} + +/// `dh/dt` (the source's `dg/dt`) `= F̄·d_r + Ḡ·d_t + H̄·d_n` with +/// (Eq. A.1, renamed): `F̄ = −√(p/µ)·cos l`, +/// `Ḡ = √(p/µ)·(h + (1+W)·sin l)/W`, `H̄ = −√(p/µ)·k·χ/W` +/// (source `H̄ = √(p/µ)·f·(h·sin l − k·cos l)/W`, and `q·sin l − p·cos l = −χ`; +/// this is the Roth-sign-corrected form). +fn rate_h(aux: &GveAux, d_r: f64, d_t: f64, d_n: f64) -> f64 { + let s = aux.sqrt_p_mu; + let f_bar = -s * aux.cos_l; + let g_bar = s * (aux.h + (1.0 + aux.w) * aux.sin_l) / aux.w; + let h_bar = -s * aux.k * aux.chi() / aux.w; + f_bar * d_r + g_bar * d_t + h_bar * d_n +} + +/// `dk/dt` (the source's `df/dt`) `= C̄·d_r + D̄·d_t + Ē·d_n` with +/// (Eq. A.1, renamed): `C̄ = √(p/µ)·sin l`, +/// `D̄ = √(p/µ)·(k + (1+W)·cos l)/W`, `Ē = √(p/µ)·h·χ/W` +/// (source `Ē = √(p/µ)·g·(k·cos l − h·sin l)/W`, and `p·cos l − q·sin l = χ`). +fn rate_k(aux: &GveAux, d_r: f64, d_t: f64, d_n: f64) -> f64 { + let s = aux.sqrt_p_mu; + let c_bar = s * aux.sin_l; + let d_bar = s * (aux.k + (1.0 + aux.w) * aux.cos_l) / aux.w; + let e_bar = s * aux.h * aux.chi() / aux.w; + c_bar * d_r + d_bar * d_t + e_bar * d_n +} + +/// `dp/dt` (the source's `dk/dt`) `= J̄·d_n` with (Eq. A.1, renamed): +/// `J̄ = √(p/µ)·(1 + p² + q²)·sin l / (2W)` (source `(1 + h² + k²)` — the +/// inclination-element magnitude `1 + tan²(i/2) = sec²(i/2)`, never singular +/// for prograde orbits). +fn rate_p(aux: &GveAux, d_n: f64) -> f64 { + let s2 = 1.0 + aux.p * aux.p + aux.q * aux.q; + aux.sqrt_p_mu * s2 * aux.sin_l / (2.0 * aux.w) * d_n +} + +/// `dq/dt` (the source's `dh/dt`) `= Ī·d_n` with (Eq. A.1, renamed): +/// `Ī = √(p/µ)·(1 + p² + q²)·cos l / (2W)`. +fn rate_q(aux: &GveAux, d_n: f64) -> f64 { + let s2 = 1.0 + aux.p * aux.p + aux.q * aux.q; + aux.sqrt_p_mu * s2 * aux.cos_l / (2.0 * aux.w) * d_n +} + +/// `dλ/dt = K̄·d_r + L̄·d_t + M̄·d_n + n` with (Eq. A.1, renamed) the +/// two-body secular mean motion `n = √(µ/a³)` and +/// `K̄ = −√(p/µ)·((W−1)/(1+η) + 2η/W)`, +/// `L̄ = −L·(1+W)·(h·cos l − k·sin l)/(µ·W·(1+η))` +/// (source `(g·cos l − f·sin l)`), `M̄ = −L·χ/(µ·W)` +/// (source `−L·(k·cos l − h·sin l)/(µ·W)`). +fn rate_lambda(aux: &GveAux, d_r: f64, d_t: f64, d_n: f64) -> f64 { + let s = aux.sqrt_p_mu; + let k_bar = -s * ((aux.w - 1.0) / (1.0 + aux.eta) + 2.0 * aux.eta / aux.w); + let l_bar = -aux.ang_mom * (1.0 + aux.w) * (aux.h * aux.cos_l - aux.k * aux.sin_l) + / (aux.mu * aux.w * (1.0 + aux.eta)); + let m_bar = -aux.ang_mom * aux.chi() / (aux.mu * aux.w); + k_bar * d_r + l_bar * d_t + m_bar * d_n + aux.n +} + +// --------------------------------------------------------------------------- +// Element-rate evaluator and RK4 propagation +// --------------------------------------------------------------------------- + +/// Evaluate `d[a, h, k, p, q, λ]/dt` at time `t` for the stored equinoctial +/// set under a time-aware inertial perturbing-acceleration closure. +/// +/// The closure `perturbation(t, &r, &v)` returns the **perturbing** inertial +/// acceleration (two-body excluded — see [`j2_perturbation_acceleration`]); +/// it is rotated into RSW internally. With a zero closure, only `dλ/dt = n` +/// is nonzero (pure two-body secular motion). +/// +/// Domain: valid for bound, prograde orbits (`0 ≤ e < 1`, `0 ≤ i < π`); +/// nonsingular at `e = 0` and `i = 0` (`W → 1`, `η → 1`, `1 + p² + q² → 1`). +pub fn equinoctial_element_rates( + elements: &[f64; 6], + mu: f64, + t: f64, + perturbation: &F, +) -> [f64; 6] +where + F: Fn(f64, &Vector3, &Vector3) -> Vector3, +{ + let aux = GveAux::from_elements(elements, mu); + let a_pert = perturbation(t, &aux.r, &aux.v); + let basis = RswBasis::from_state(&aux.r, &aux.v); + let (d_r, d_t, d_n) = basis.project(&a_pert); + [ + rate_a(&aux, d_r, d_t), + rate_h(&aux, d_r, d_t, d_n), + rate_k(&aux, d_r, d_t, d_n), + rate_p(&aux, d_n), + rate_q(&aux, d_n), + rate_lambda(&aux, d_r, d_t, d_n), + ] +} + +/// Number of equal RK4 sub-steps covering `dt`: `ceil(dt / max_step_s)`, at +/// least 1 (so `dt = 0` degenerates to a single identity step). Mirrors the +/// `KeplerJ2` Cartesian model's sub-stepping so the two formulations share a +/// step convention. +fn substep_count(dt: f64, max_step_s: f64) -> usize { + (dt / max_step_s).ceil().max(1.0) as usize +} + +/// `x + a·y` element-wise for the 6-vector of elements (RK4 stage state). +fn axpy(x: &[f64; 6], a: f64, y: &[f64; 6]) -> [f64; 6] { + let mut out = *x; + for i in 0..6 { + out[i] += a * y[i]; + } + out +} + +/// One classical fixed-step RK4 step of the first-order element ODE +/// `dx/dt = f(t, x)` over `dt`, starting at time `t`. +fn rk4_element_step(x: &[f64; 6], mu: f64, t: f64, dt: f64, perturbation: &F) -> [f64; 6] +where + F: Fn(f64, &Vector3, &Vector3) -> Vector3, +{ + let half = 0.5 * dt; + let k1 = equinoctial_element_rates(x, mu, t, perturbation); + let k2 = equinoctial_element_rates(&axpy(x, half, &k1), mu, t + half, perturbation); + let k3 = equinoctial_element_rates(&axpy(x, half, &k2), mu, t + half, perturbation); + let k4 = equinoctial_element_rates(&axpy(x, dt, &k3), mu, t + dt, perturbation); + let mut out = *x; + for i in 0..6 { + out[i] += dt / 6.0 * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]); + } + out +} + +/// Propagate the stored equinoctial elements over `dt` starting at time `t0` +/// with sub-stepped fixed RK4 (`ceil(dt / max_step_s)` equal sub-steps) over +/// the element rates, under the given inertial perturbation closure. +/// +/// The mean longitude accumulates continuously (never reduced mod 2π here — +/// design Decision 3's convention; the element→Cartesian conversion owns +/// periodicity). Deterministic: identical inputs yield bitwise-identical +/// outputs. +pub fn propagate_equinoctial( + elements: [f64; 6], + mu: f64, + t0: f64, + dt: f64, + max_step_s: f64, + perturbation: &F, +) -> [f64; 6] +where + F: Fn(f64, &Vector3, &Vector3) -> Vector3, +{ + let steps = substep_count(dt, max_step_s); + let sub_dt = dt / steps as f64; + let mut x = elements; + let mut t = t0; + for _ in 0..steps { + x = rk4_element_step(&x, mu, t, sub_dt, perturbation); + t += sub_dt; + } + x +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::orbital::integrate::rk4_step; + use std::f64::consts::TAU; + + const EARTH: GravityModel = GravityModel::EARTH_WGS84; + const EARTH_MU: f64 = GravityModel::EARTH_WGS84.mu; + + /// The J2-only perturbation closure used throughout: pure oblateness, + /// two-body excluded (design Decision 2). Time-independent for J2. + fn j2_closure( + gravity: GravityModel, + ) -> impl Fn(f64, &Vector3, &Vector3) -> Vector3 { + move |_t, r, _v| j2_perturbation_acceleration(r, &gravity) + } + + /// Zero perturbation closure (pure two-body). + fn zero_closure() -> impl Fn(f64, &Vector3, &Vector3) -> Vector3 { + |_t, _r, _v| Vector3::zeros() + } + + fn equinoctial_state(elements: &[f64; 6], mu: f64) -> OrbitalState { + OrbitalState { + elements: OrbitalElements::Equinoctial { + sma: elements[0], + h: elements[1], + k: elements[2], + p: elements[3], + q: elements[4], + mean_longitude: elements[5], + }, + mu, + frame: Frame::Teme, + epoch: Epoch::from_jde_utc(CONVERSION_EPOCH_JD), + } + } + + /// Equinoctial elements `[a, h, k, p, q, λ]` for a Keplerian state. + fn elements_from_keplerian( + sma: f64, + ecc: f64, + inc: f64, + raan: f64, + argp: f64, + true_anomaly: f64, + mu: f64, + ) -> [f64; 6] { + let state = OrbitalState { + elements: OrbitalElements::Keplerian { + sma, + ecc, + inc, + raan, + argp, + true_anomaly, + }, + mu, + frame: Frame::Teme, + epoch: Epoch::from_jde_utc(CONVERSION_EPOCH_JD), + }; + let (a, h, k, p, q, l) = state.as_equinoctial().unwrap(); + [a, h, k, p, q, l] + } + + fn elements_to_position(elements: &[f64; 6], mu: f64) -> Vector3 { + equinoctial_state(elements, mu).as_cartesian().unwrap().0 + } + + /// Cartesian two-body+J2 reference propagator: sub-stepped RK4 over the + /// shared `j2_acceleration` (two-body + J2 composite) closure — the + /// independent formulation the GVE path is validated against. + fn propagate_cartesian( + r0: Vector3, + v0: Vector3, + gravity: GravityModel, + dt: f64, + max_step_s: f64, + ) -> (Vector3, Vector3) { + let steps = substep_count(dt, max_step_s); + let sub_dt = dt / steps as f64; + let accel = |p: &Vector3, _v: &Vector3| j2_acceleration(p, &gravity); + let (mut r, mut v) = (r0, v0); + for _ in 0..steps { + let (rr, vv) = rk4_step(&r, &v, sub_dt, accel); + r = rr; + v = vv; + } + (r, v) + } + + // ── RSW rotation (spec: "RSW rotation is orthonormal and right-handed") ── + + #[test] + fn rsw_basis_is_orthonormal_and_right_handed() { + // A generic inclined, eccentric state: all components nonzero. + let elements = elements_from_keplerian(9_000_000.0, 0.3, 1.1, 0.7, 0.9, 1.3, EARTH_MU); + let (r, v) = equinoctial_state(&elements, EARTH_MU) + .as_cartesian() + .unwrap(); + let basis = RswBasis::from_state(&r, &v); + + // Unit length. + for u in [basis.radial, basis.transverse, basis.normal] { + assert!((u.norm() - 1.0).abs() < 1e-15, "not unit: {}", u.norm()); + } + // Mutually orthogonal. + assert!(basis.radial.dot(&basis.transverse).abs() < 1e-15); + assert!(basis.radial.dot(&basis.normal).abs() < 1e-15); + assert!(basis.transverse.dot(&basis.normal).abs() < 1e-15); + // W is parallel to the angular-momentum vector r×v. + let h_vec = r.cross(&v); + assert!((basis.normal - h_vec.normalize()).norm() < 1e-15); + // Right-handed: R̂ × Ŝ = Ŵ. + assert!((basis.radial.cross(&basis.transverse) - basis.normal).norm() < 1e-15); + // Transverse has a positive velocity projection (points "forward"). + assert!(basis.transverse.dot(&v) > 0.0); + } + + // ── Unperturbed motion (spec: "Unperturbed motion moves only λ") ──────── + + #[test] + fn unperturbed_motion_moves_only_mean_longitude() { + let elements = elements_from_keplerian(7_500_000.0, 0.2, 0.6, 1.0, 0.4, 2.0, EARTH_MU); + let rates = equinoctial_element_rates(&elements, EARTH_MU, 0.0, &zero_closure()); + + // a, h, k, p, q rates are products with a zero perturbation ⇒ exactly 0. + for (i, label) in ["a", "h", "k", "p", "q"].iter().enumerate() { + assert_eq!(rates[i], 0.0, "d{label}/dt must be exactly zero"); + } + // dλ/dt is exactly the two-body mean motion n = √(µ/a³). + let n = (EARTH_MU / elements[0].powi(3)).sqrt(); + assert!( + (rates[5] - n).abs() < 1e-15 * n, + "dλ/dt = {} vs n = {n}", + rates[5] + ); + } + + // ── Structural spot values (spec: "Published spot values reproduced") ─── + // + // The fetched source (arXiv:2105.06516, Appendix A) states the equinoctial + // GVE symbolically — it carries NO worked numeric example — so, per the + // spec's explicit allowance, structural identities derived from the fetched + // equations are verified instead of a pinned numeric value. The chosen + // identities are the classical Gauss-planetary reductions at zero + // eccentricity, which the transcribed coefficients must reproduce exactly: + // • tangential accel on a circular orbit: da/dt = (2/n)·d_t + // (source B̄ = 2a²W/L → 2a²/√(µa) = 2/n at e=0, W=1). + // • radial accel on a circular orbit: dλ/dt − n = −(2/(n·a))·d_r + // (source K̄ = −√(p/µ)·((W−1)/(1+η) + 2η/W) → −2√(a/µ) = −2/(n·a)). + #[test] + fn circular_orbit_reductions_match_classical_gauss() { + // Circular (e = 0 ⇒ h = k = 0), inclined orbit; λ arbitrary. + let a = 8_000_000.0; + let elements = elements_from_keplerian(a, 0.0, 0.7, 0.5, 0.0, 1.7, EARTH_MU); + let aux = GveAux::from_elements(&elements, EARTH_MU); + // e = 0 ⇒ W = 1, η = 1 exactly. + assert!((aux.w - 1.0).abs() < 1e-12, "W = {}", aux.w); + assert!((aux.eta - 1.0).abs() < 1e-12, "η = {}", aux.eta); + + let n = aux.n; + // Pure tangential unit acceleration. + let da_dt = rate_a(&aux, 0.0, 1.0); + assert!( + (da_dt - 2.0 / n).abs() < 1e-6 * (2.0 / n), + "da/dt = {da_dt} vs 2/n = {}", + 2.0 / n + ); + // Pure radial unit acceleration: dλ/dt − n = K̄ = −2/(n·a). + let dlam_dt = rate_lambda(&aux, 1.0, 0.0, 0.0); + let expected = -2.0 / (n * a); + assert!( + (dlam_dt - n - expected).abs() < 1e-6 * expected.abs(), + "K̄ = {} vs −2/(na) = {expected}", + dlam_dt - n + ); + } + + // ── Near-singular domain (spec: "Near-circular near-equatorial finite") ─ + + #[test] + fn near_circular_near_equatorial_evaluation_is_finite() { + // e = 1e-8, i = 1e-8: h,k ~ 1e-8, p,q ~ 5e-9. + let elements = elements_from_keplerian(7_000_000.0, 1e-8, 1e-8, 0.0, 0.0, 0.9, EARTH_MU); + assert!(elements[1].hypot(elements[2]) < 1e-7, "e not tiny"); + let rates = equinoctial_element_rates(&elements, EARTH_MU, 0.0, &j2_closure(EARTH)); + assert!( + rates.iter().all(|r| r.is_finite()), + "non-finite rate at near-singular state: {rates:?}" + ); + // And the propagation stays finite over an arc. + let end = propagate_equinoctial(elements, EARTH_MU, 0.0, 3600.0, 10.0, &j2_closure(EARTH)); + assert!( + end.iter().all(|x| x.is_finite()), + "non-finite propagation: {end:?}" + ); + } + + // ── Deterministic propagation (spec: "Deterministic element propagation") ─ + + #[test] + fn element_propagation_is_bitwise_deterministic() { + let elements = elements_from_keplerian(7_200_000.0, 0.05, 0.9, 1.2, 0.5, 0.3, EARTH_MU); + let run = + || propagate_equinoctial(elements, EARTH_MU, 0.0, 5400.0, 10.0, &j2_closure(EARTH)); + let a = run(); + let b = run(); + for i in 0..6 { + assert_eq!( + a[i].to_bits(), + b[i].to_bits(), + "element {i} not bit-identical" + ); + } + } + + // ── Step-halving convergence (spec: "Convergence under step halving") ─── + + #[test] + fn step_halving_shows_rk4_order() { + // A J2-perturbed LEO arc (~one orbit). Reference at a very fine step; + // compare the endpoint position error at step H and H/2. + let elements = elements_from_keplerian(7_000_000.0, 0.02, 0.9, 1.0, 0.5, 0.0, EARTH_MU); + let arc = 5000.0; // s (< one LEO period) + let j2 = j2_closure(EARTH); + + let reference = propagate_equinoctial(elements, EARTH_MU, 0.0, arc, arc / 4096.0, &j2); + let ref_pos = elements_to_position(&reference, EARTH_MU); + + let coarse_h = 40.0; + let err = |h: f64| { + let end = propagate_equinoctial(elements, EARTH_MU, 0.0, arc, h, &j2); + (elements_to_position(&end, EARTH_MU) - ref_pos).norm() + }; + let err_coarse = err(coarse_h); + let err_fine = err(coarse_h / 2.0); + let ratio = err_coarse / err_fine; + + // RK4 is 4th order ⇒ halving the step shrinks the error ≈ 2⁴ = 16×. + // MEASURED (this arc/config, cargo test): err(40 s) ≈ 1.43e-3 m, + // err(20 s) ≈ 8.85e-5 m, ratio ≈ 16.13. Band tolerates the + // pre-asymptotic and round-off edges. + assert!( + (8.0..=32.0).contains(&ratio), + "step-halving ratio {ratio:.2} (err_H = {err_coarse:.3e} m, err_H/2 = {err_fine:.3e} m) not ~16" + ); + } + + // ── Cross-formulation equivalence (specs: LEO / eccentric both directions) ─ + // + // Identical J2 physics through the GVE-in-elements path and the independent + // Cartesian two-body+J2 RK4 path (they share only the J2/two-body force + // math, no element-rate code), compared in inertial position at the arc + // endpoint. "Both directions" is covered per regime: the same physical + // initial state is authored once as Keplerian → equinoctial (element-born, + // fed to the GVE path) and once as Keplerian → Cartesian (Cartesian-born, + // fed to the RK4 path); the element endpoint is converted back to Cartesian + // for the comparison, exercising the equinoctial→Cartesian direction at the + // end as well. Tolerances are MEASURED at the chosen steps and documented + // per test; they are tight enough that any term/sign error in the GVE + // (which would perturb the J2 secular/periodic signature far more than RK4 + // truncation) fails the assertion. + + /// A cross-formulation equivalence case: a Keplerian initial state + /// (`sma, ecc, inc, raan, argp, true_anomaly`) plus the arc and RK4 step. + struct EquivCase { + kep: [f64; 6], + arc: f64, + step: f64, + } + + /// Endpoint inertial-position gap (m) between the GVE-in-elements path and + /// the Cartesian two-body+J2 path for a case's initial state. + fn gve_vs_cartesian_gap(case: &EquivCase) -> f64 { + let [sma, ecc, inc, raan, argp, true_anomaly] = case.kep; + let elements = elements_from_keplerian(sma, ecc, inc, raan, argp, true_anomaly, EARTH_MU); + let (r0, v0) = equinoctial_state(&elements, EARTH_MU) + .as_cartesian() + .unwrap(); + + let el_end = propagate_equinoctial( + elements, + EARTH_MU, + 0.0, + case.arc, + case.step, + &j2_closure(EARTH), + ); + let r_gve = elements_to_position(&el_end, EARTH_MU); + + let (r_cart, _) = propagate_cartesian(r0, v0, EARTH, case.arc, case.step); + (r_gve - r_cart).norm() + } + + #[test] + fn leo_equivalence_both_directions() { + // Near-circular LEO, ~3 revolutions (period ≈ 5560 s), 5 s steps. + let period = TAU * (7_000_000_f64.powi(3) / EARTH_MU).sqrt(); + let gap = gve_vs_cartesian_gap(&EquivCase { + kep: [7_000_000.0, 0.01, 0.9, 1.2, 0.4, 0.0], + arc: 3.0 * period, + step: 5.0, + }); + // MEASURED (cargo test): gap ≈ 2.75e-3 m over 3 revs at 5 s steps + // (dominated by the Cartesian RK4 two-body truncation — the GVE + // two-body part is analytic). Tolerance 0.1 m. + assert!(gap < 0.1, "LEO GVE vs Cartesian gap = {gap:.4e} m"); + } + + #[test] + fn meo_equivalence() { + // MEO (a ≈ 20 000 km, mild eccentricity), ~2 revolutions, 15 s steps. + let period = TAU * (20_000_000_f64.powi(3) / EARTH_MU).sqrt(); + let gap = gve_vs_cartesian_gap(&EquivCase { + kep: [20_000_000.0, 0.1, 0.7, 0.5, 1.1, 0.0], + arc: 2.0 * period, + step: 15.0, + }); + // MEASURED (cargo test): gap ≈ 8.2e-4 m. Tolerance 0.1 m. + assert!(gap < 0.1, "MEO GVE vs Cartesian gap = {gap:.4e} m"); + } + + #[test] + fn eccentric_equivalence_through_perigee() { + // Eccentric e = 0.6 (in the documented 0.3–0.7 band), a = 20 000 km ⇒ + // perigee ≈ 8000 km (above the surface), through ~3 perigee passages. + // 2 s steps to resolve the fast perigee dynamics in both formulations. + let period = TAU * (20_000_000_f64.powi(3) / EARTH_MU).sqrt(); + let gap = gve_vs_cartesian_gap(&EquivCase { + kep: [20_000_000.0, 0.6, 0.5, 0.9, 0.3, 0.0], + arc: 3.0 * period, + step: 2.0, + }); + // MEASURED (cargo test): gap ≈ 3.0e-4 m over 3 perigee passages at 2 s + // steps — the fast-variable stress case; with 2 s steps both + // formulations resolve perigee tightly, so they agree to sub-mm. + // Tolerance 0.1 m (a wrong GVE term would drift kilometres — caught + // with ~300× margin). + assert!(gap < 0.1, "eccentric GVE vs Cartesian gap = {gap:.4e} m"); + } +} diff --git a/crates/thresh-core/src/orbital/mod.rs b/crates/thresh-core/src/orbital/mod.rs index 0958eed..c43f749 100644 --- a/crates/thresh-core/src/orbital/mod.rs +++ b/crates/thresh-core/src/orbital/mod.rs @@ -24,6 +24,7 @@ pub mod egm96; pub mod ephemeris; pub mod force_config; pub mod gravity; +pub mod gve; pub mod integrate; pub mod srp; pub mod state; diff --git a/crates/thresh-eval/tests/orbital_coast_gap.rs b/crates/thresh-eval/tests/orbital_coast_gap.rs new file mode 100644 index 0000000..df22dee --- /dev/null +++ b/crates/thresh-eval/tests/orbital_coast_gap.rs @@ -0,0 +1,463 @@ +//! Coast-gap ANEES demonstration (element-space-orbital-filtering tasks 4.1–4.2, +//! design Decision 5). +//! +//! A seeded Monte-Carlo experiment that measures the change's payoff. Identical +//! deterministic truth arcs (Cartesian two-body+J2) are tracked with identical +//! ECI-position measurements under a **shared physical process-noise assumption** +//! (one isotropic acceleration PSD `σ_accel`, mapped into each filter's +//! coordinates — `KeplerJ2`'s Cartesian CWNA block and the equinoctial GVE +//! input-matrix map are the two forms of the *same* Stacey & D'Amico Eq. 5; +//! see `EquinoctialModel`'s docs). After a warmup, both filters coast through +//! measurement-free gaps of growing duration; at each gap end the +//! position-marginal ANEES (dof 3, in the common ECI position space) is measured +//! over `RUNS` seeded runs and judged against the two-sided 95% χ² band. +//! +//! # What the experiment isolates (design Decision 5) +//! +//! Decision 5's claim is specifically about **coast** — measurement-free +//! covariance propagation: element-space uncertainty stays Gaussian for many +//! revolutions while the Cartesian along-track uncertainty curves into a +//! "banana" that a linearized (EKF) covariance cannot represent. So the warmup +//! establishes one shared, consistent posterior (via the Cartesian EKF that is +//! also the baseline), and at gap start that posterior is expressed in each +//! filter's coordinates — kept as-is for the Cartesian EKF, and mapped to +//! equinoctial elements by an unscented transform for the equinoctial UKF (which +//! the diagnostics behind this test confirmed is consistent: element-space NEES +//! ≈ dof at gap start). Both then coast predict-only. This deliberately keeps the +//! comparison to the coast-phase covariance propagation the change is about, and +//! away from the (separate, known) trade-off that filtering ECI **position** +//! measurements — linear in Cartesian, nonlinear in elements — is statistically +//! harder in element space; that measurement-update behaviour is out of this +//! demonstration's scope. Truth is propagated by the very `KeplerJ2::predict` +//! the EKF uses (zero dynamics model error there); the equinoctial model matches +//! it to the sub-metre cross-formulation tolerance over the swept horizons, +//! negligible against the metre-to-kilometre coast covariance. +//! +//! # Result (the recorded sweep, RUNS = 50, seed = 7) +//! +//! The Cartesian EKF's along-track banana makes its position ANEES exceed the +//! band once per revolution (the banana's major axis rotating with the orbit), +//! while the equinoctial UKF stays inside the band throughout. Two clean +//! crossover gaps (Cartesian out, equinoctial in): **2.39 rev** (EKF 4.76, +//! band ≈ [2.36, 3.72], UKF 2.85) and **3.35 rev** (EKF 4.37, UKF 2.89). The +//! full sweep is printed by the test and asserted below. + +use nalgebra::{DMatrix, DVector, Vector3}; +use rand::prelude::*; + +use thresh_eval::consistency::{ConsistencyAccumulator, ConsistencyVerdict, chi2, nees}; +use thresh_filter::ekf::ExtendedKalmanFilter; +use thresh_filter::models::equinoctial::{EquinoctialModel, equinoctial_position}; +use thresh_filter::models::kepler_j2::KeplerJ2; +use thresh_filter::traits::MotionModel; +use thresh_filter::ukf::{UkfParams, UnscentedKalmanFilter}; + +use thresh_core::orbital::{ + Frame, GravityModel, OrbitalElements, OrbitalState, keplerian_to_cartesian, +}; +use thresh_core::time::Epoch; + +// ── Scenario constants (the resolved sweep — recorded in design.md, task 4.1) ─ + +const EARTH: GravityModel = GravityModel::EARTH_WGS84; +const MU: f64 = GravityModel::EARTH_WGS84.mu; +/// Fixed J2000 epoch for the pure-geometry element conversions. +const CONVERSION_EPOCH_JD: f64 = 2_451_545.0; + +/// Predict / measurement cadence (s). +const DT_STEP: f64 = 60.0; +/// RK4 sub-step ceiling shared by truth and both filters (s). At 15 s the +/// equinoctial GVE path matches the Cartesian truth to well under a metre over +/// the swept horizons — negligible against the coast covariance. +const MAX_STEP_S: f64 = 15.0; +/// Warmup measurement updates establishing the shared posterior. +const WARMUP_STEPS: usize = 5; +/// Monte-Carlo runs per gap (ANEES sample count at each checkpoint). +const RUNS: usize = 50; +/// Initial 1σ position uncertainty per axis (m). +const SIGMA_R: f64 = 3000.0; +/// Initial 1σ velocity uncertainty per axis (m/s) — large, so the along-track +/// banana forms within a few revolutions (the coast regime Decision 5 targets). +const SIGMA_V: f64 = 60.0; +/// ECI-position measurement 1σ per axis (m). Loose warmup keeps the shared +/// posterior large enough that the banana breaks the Cartesian EKF in a few +/// revolutions. +const SIGMA_Z: f64 = 200.0; +/// Shared isotropic acceleration white-noise PSD (m/s²·Hz^-½). +const SIGMA_ACCEL: f64 = 1e-6; +/// Fixed seed — the demonstration is deterministic, not statistical luck. +const SEED: u64 = 7; + +/// Coast-gap checkpoints in predict steps (0 = gap start, at warmup end). The +/// LEO period is ≈ 92.7 steps of 60 s, so this sweeps 0 → ~4 revolutions at +/// roughly half-revolution spacing. +const CHECKPOINTS: [usize; 9] = [0, 46, 93, 139, 185, 232, 278, 325, 371]; + +// ── Truth orbit and small conversion helpers ───────────────────────────────── + +/// Interleaved `[x, vx, y, vy, z, vz]` initial truth state: LEO, a = 7000 km, +/// e = 0.001, i = 51.6°. +fn truth0() -> DVector { + let (r, v) = + keplerian_to_cartesian(7_000_000.0, 0.001, 51.6_f64.to_radians(), 0.3, 0.4, 0.5, MU); + DVector::from_row_slice(&[r.x, v.x, r.y, v.y, r.z, v.z]) +} + +fn to_posvel(x: &DVector) -> (Vector3, Vector3) { + ( + Vector3::new(x[0], x[2], x[4]), + Vector3::new(x[1], x[3], x[5]), + ) +} + +/// Deterministic Cartesian two-body+J2 truth, propagated by the same +/// `KeplerJ2::predict` the Cartesian EKF uses (so that filter carries zero +/// dynamics model error). Returns state at every step time `i·DT_STEP`. +fn build_truth() -> Vec<(Vector3, Vector3)> { + let model = KeplerJ2 { + gravity: EARTH, + max_step_s: MAX_STEP_S, + sigma_accel: 0.0, + }; + let total = WARMUP_STEPS + CHECKPOINTS[CHECKPOINTS.len() - 1]; + let mut x = truth0(); + let mut out = Vec::with_capacity(total + 1); + out.push(to_posvel(&x)); + for _ in 0..total { + x = model.predict(&x, DT_STEP); + out.push(to_posvel(&x)); + } + out +} + +/// Interleaved Cartesian state → equinoctial element vector `[a, h, k, p, q, λ]`. +fn cartesian_to_equinoctial_vec(cart: &DVector) -> DVector { + let (r, v) = to_posvel(cart); + let state = OrbitalState { + elements: OrbitalElements::Cartesian { + position: r, + velocity: v, + }, + mu: MU, + frame: Frame::Teme, + epoch: Epoch::from_jde_utc(CONVERSION_EPOCH_JD), + }; + let (a, h, k, p, q, l) = state.as_equinoctial().unwrap(); + DVector::from_row_slice(&[a, h, k, p, q, l]) +} + +/// Standard normal via Box–Muller on the seeded uniform stream. +fn gauss(rng: &mut StdRng) -> f64 { + let u1: f64 = rng.random(); + let u2: f64 = rng.random(); + (-2.0 * (1.0 - u1).ln()).sqrt() * (std::f64::consts::TAU * u2).cos() +} + +// ── Unscented transform (Van der Merwe), used for the element filter's +// initialization from the shared posterior and for the element→position +// projection at NEES time ──────────────────────────────────────────────── + +/// Unscented transform of `N(mean, cov)` through `f`, returning the mapped mean +/// and covariance. `alpha = 1`, `beta = 2`, `kappa = 0`: exact for a linear +/// `f`, second-order for a nonlinear one. +fn unscented_transform( + mean: &DVector, + cov: &DMatrix, + f: F, +) -> (DVector, DMatrix) +where + F: Fn(&DVector) -> DVector, +{ + let n = mean.len(); + let (alpha, beta, kappa) = (1.0_f64, 2.0_f64, 0.0_f64); + let lambda = alpha * alpha * (n as f64 + kappa) - n as f64; + let scale = n as f64 + lambda; + let l = (cov * scale) + .cholesky() + .expect("UT covariance is positive definite") + .l(); + + let mut points = Vec::with_capacity(2 * n + 1); + points.push(mean.clone()); + for i in 0..n { + let col = l.column(i).clone_owned(); + points.push(mean + &col); + points.push(mean - &col); + } + let mapped: Vec> = points.iter().map(&f).collect(); + + let wm0 = lambda / scale; + let wc0 = wm0 + (1.0 - alpha * alpha + beta); + let w = 1.0 / (2.0 * scale); + + let mut out_mean = wm0 * &mapped[0]; + for point in &mapped[1..] { + out_mean += w * point; + } + let d0 = &mapped[0] - &out_mean; + let mut out_cov = wc0 * &d0 * d0.transpose(); + for point in &mapped[1..] { + let d = point - &out_mean; + out_cov += w * &d * d.transpose(); + } + (out_mean, out_cov) +} + +// ── Per-run sampling and the shared-warmup coast ───────────────────────────── + +/// Diagonal interleaved initial Cartesian covariance `diag(σ_r², σ_v², …)`. +fn p0_cartesian() -> DMatrix { + DMatrix::from_diagonal(&DVector::from_row_slice(&[ + SIGMA_R * SIGMA_R, + SIGMA_V * SIGMA_V, + SIGMA_R * SIGMA_R, + SIGMA_V * SIGMA_V, + SIGMA_R * SIGMA_R, + SIGMA_V * SIGMA_V, + ])) +} + +/// Sample one run's initial Cartesian estimate `N(truth0, P0)` and its warmup +/// measurement sequence (`truth_pos + N(0, σ_z²I)`). +fn sample_run( + rng: &mut StdRng, + truth: &[(Vector3, Vector3)], +) -> (DVector, Vec>) { + let (r0, v0) = truth[0]; + let x0 = DVector::from_row_slice(&[ + r0.x + SIGMA_R * gauss(rng), + v0.x + SIGMA_V * gauss(rng), + r0.y + SIGMA_R * gauss(rng), + v0.y + SIGMA_V * gauss(rng), + r0.z + SIGMA_R * gauss(rng), + v0.z + SIGMA_V * gauss(rng), + ]); + let mut meas = Vec::with_capacity(WARMUP_STEPS); + for k in 0..WARMUP_STEPS { + let p = truth[k + 1].0; + meas.push(DVector::from_row_slice(&[ + p.x + SIGMA_Z * gauss(rng), + p.y + SIGMA_Z * gauss(rng), + p.z + SIGMA_Z * gauss(rng), + ])); + } + (x0, meas) +} + +/// 3×6 position selector for the interleaved Cartesian state. +fn position_selector() -> DMatrix { + DMatrix::from_row_slice( + 3, + 6, + &[ + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, // + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, // + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + ], + ) +} + +fn measurement_noise() -> DMatrix { + DMatrix::identity(3, 3) * (SIGMA_Z * SIGMA_Z) +} + +fn unpack6(state: &DVector) -> [f64; 6] { + [state[0], state[1], state[2], state[3], state[4], state[5]] +} + +/// Position-marginal NEES (dof 3) of the interleaved Cartesian EKF against a +/// truth position: error `truth − estimate`, marginal 3×3 position block of P. +fn cartesian_position_nees(ekf: &ExtendedKalmanFilter, truth_pos: &Vector3) -> f64 { + let idx = [0usize, 2, 4]; + let error = DVector::from_row_slice(&[ + truth_pos.x - ekf.x[0], + truth_pos.y - ekf.x[2], + truth_pos.z - ekf.x[4], + ]); + let p_pos = DMatrix::from_fn(3, 3, |i, j| ekf.p[(idx[i], idx[j])]); + nees(&error, &p_pos).expect("Cartesian position block invertible") +} + +/// Position-marginal NEES (dof 3) of the element UKF against a truth position: +/// the element (mean, covariance) are projected into ECI position space by the +/// unscented transform, then scored — the same Gaussian projection the linear +/// Cartesian marginal block is for the EKF. +fn element_position_nees(ukf: &UnscentedKalmanFilter, truth_pos: &Vector3) -> f64 { + let (pos_mean, p_pos) = + unscented_transform(&ukf.x, &ukf.p, |e| equinoctial_position(&unpack6(e), MU)); + let error = DVector::from_row_slice(&[ + truth_pos.x - pos_mean[0], + truth_pos.y - pos_mean[1], + truth_pos.z - pos_mean[2], + ]); + nees(&error, &p_pos).expect("element position projection invertible") +} + +/// One seeded run: shared Cartesian warmup, then coast both filters predict-only, +/// recording position NEES at each checkpoint. Returns +/// `(cartesian_nees_per_checkpoint, equinoctial_nees_per_checkpoint)`. +fn coast_run( + rng: &mut StdRng, + truth: &[(Vector3, Vector3)], + reference_elements: [f64; 6], +) -> (Vec, Vec) { + let (x0, meas) = sample_run(rng, truth); + let kepler = KeplerJ2 { + gravity: EARTH, + max_step_s: MAX_STEP_S, + sigma_accel: SIGMA_ACCEL, + }; + let equinoctial = EquinoctialModel { + gravity: EARTH, + max_step_s: MAX_STEP_S, + sigma_accel: SIGMA_ACCEL, + reference_elements, + }; + let h = position_selector(); + let r = measurement_noise(); + + // Shared warmup: the Cartesian EKF establishes the posterior at gap start. + let mut ekf = ExtendedKalmanFilter::new(x0, p0_cartesian()); + for z in &meas { + ekf.predict(&kepler, DT_STEP); + ekf.update_linear(z, &h, &r); + } + // The equinoctial UKF starts from that same posterior, mapped to elements. + let (x0e, p0e) = unscented_transform(&ekf.x, &ekf.p, cartesian_to_equinoctial_vec); + let mut ukf = UnscentedKalmanFilter::new( + x0e, + p0e, + UkfParams { + alpha: 1.0, + beta: 2.0, + kappa: 0.0, + }, + ); + + let mut cart_nees = Vec::with_capacity(CHECKPOINTS.len()); + let mut equi_nees = Vec::with_capacity(CHECKPOINTS.len()); + let max_coast = CHECKPOINTS[CHECKPOINTS.len() - 1]; + for c in 0..=max_coast { + if c > 0 { + ekf.predict(&kepler, DT_STEP); + ukf.predict(&equinoctial, DT_STEP); + } + if CHECKPOINTS.contains(&c) { + let truth_pos = &truth[WARMUP_STEPS + c].0; + cart_nees.push(cartesian_position_nees(&ekf, truth_pos)); + equi_nees.push(element_position_nees(&ukf, truth_pos)); + } + } + (cart_nees, equi_nees) +} + +/// Sweep: accumulate per-checkpoint position ANEES for both filters over `RUNS` +/// seeded runs. Returns `(cartesian_accumulators, equinoctial_accumulators)`. +fn run_sweep( + truth: &[(Vector3, Vector3)], + reference_elements: [f64; 6], +) -> (Vec, Vec) { + let mut rng = StdRng::seed_from_u64(SEED); + let mut cart_accs: Vec = CHECKPOINTS + .iter() + .map(|_| ConsistencyAccumulator::new(3)) + .collect(); + let mut equi_accs: Vec = cart_accs.clone(); + for _ in 0..RUNS { + let (cart_nees, equi_nees) = coast_run(&mut rng, truth, reference_elements); + for i in 0..CHECKPOINTS.len() { + cart_accs[i].push(cart_nees[i]); + equi_accs[i].push(equi_nees[i]); + } + } + (cart_accs, equi_accs) +} + +fn reference_elements(truth: &[(Vector3, Vector3)]) -> [f64; 6] { + let (r, v) = truth[0]; + let cart = DVector::from_row_slice(&[r.x, v.x, r.y, v.y, r.z, v.z]); + let e = cartesian_to_equinoctial_vec(&cart); + [e[0], e[1], e[2], e[3], e[4], e[5]] +} + +// ── The demonstration test ────────────────────────────────────────────────── + +#[test] +fn coast_gap_element_filter_outlasts_cartesian() { + let truth = build_truth(); + let ref_elem = reference_elements(&truth); + let (cart_accs, equi_accs) = run_sweep(&truth, ref_elem); + let period = std::f64::consts::TAU * (7_000_000_f64.powi(3) / MU).sqrt(); + + // Full sweep table (spec: "full sweep values recorded at the test"). + eprintln!( + "coast-gap sweep: RUNS={RUNS}, dof=3, seed={SEED}, σ_r={SIGMA_R} m, σ_v={SIGMA_V} m/s, \ + σ_z={SIGMA_Z} m, σ_accel={SIGMA_ACCEL:e}, cadence={DT_STEP} s, warmup={WARMUP_STEPS}" + ); + eprintln!( + "{:>8} {:>6} {:>10} {:>14} {:>10} {:>14} {:>18}", + "gap_s", "rev", "EKF_ANEES", "EKF_verdict", "UKF_ANEES", "UKF_verdict", "band" + ); + let mut crossover = None; + for i in 0..CHECKPOINTS.len() { + let gap_s = CHECKPOINTS[i] as f64 * DT_STEP; + let ekf_mean = cart_accs[i].mean().unwrap(); + let ukf_mean = equi_accs[i].mean().unwrap(); + let ekf_v = cart_accs[i].verdict(chi2::DEFAULT_ALPHA).unwrap(); + let ukf_v = equi_accs[i].verdict(chi2::DEFAULT_ALPHA).unwrap(); + let (lo, hi) = cart_accs[i].bounds(chi2::DEFAULT_ALPHA).unwrap(); + eprintln!( + "{gap_s:>8.0} {:>6.2} {ekf_mean:>10.4} {:>14} {ukf_mean:>10.4} {:>14} {:>8}", + gap_s / period, + format!("{ekf_v:?}"), + format!("{ukf_v:?}"), + format!("[{lo:.3},{hi:.3}]"), + ); + if crossover.is_none() + && ekf_v != ConsistencyVerdict::Consistent + && ukf_v == ConsistencyVerdict::Consistent + { + crossover = Some((gap_s / period, ekf_mean, ukf_mean, lo, hi)); + } + } + + // Spec "Element filter outlasts the Cartesian filter through coast": a + // recorded gap where the Cartesian ANEES is out of band while the element + // ANEES is inside, with both values recorded. + let (rev, ekf_mean, ukf_mean, lo, hi) = + crossover.expect("no gap found where Cartesian is out of band and element is in band"); + eprintln!( + "CROSSOVER at {rev:.2} rev: Cartesian ANEES {ekf_mean:.4} out of band [{lo:.4}, {hi:.4}] \ + (margin {:.4} above upper); equinoctial ANEES {ukf_mean:.4} inside (margin {:.4} above \ + lower, {:.4} below upper).", + ekf_mean - hi, + ukf_mean - lo, + hi - ukf_mean + ); + assert!( + ekf_mean > hi, + "Cartesian ANEES {ekf_mean} must exceed upper bound {hi}" + ); + assert!( + ukf_mean >= lo && ukf_mean <= hi, + "equinoctial ANEES {ukf_mean} must be inside [{lo}, {hi}]" + ); + + // Spec "Demonstration is deterministic": a second seeded sweep reproduces + // every accumulated ANEES bit-for-bit. + let (cart2, equi2) = run_sweep(&truth, ref_elem); + for i in 0..CHECKPOINTS.len() { + assert_eq!( + cart_accs[i].sum.to_bits(), + cart2[i].sum.to_bits(), + "EKF sum {i}" + ); + assert_eq!( + equi_accs[i].sum.to_bits(), + equi2[i].sum.to_bits(), + "UKF sum {i}" + ); + assert_eq!(cart_accs[i].n, cart2[i].n); + assert_eq!(equi_accs[i].n, equi2[i].n); + } +} diff --git a/crates/thresh-filter/src/models.rs b/crates/thresh-filter/src/models.rs index 7d1b243..afc893c 100644 --- a/crates/thresh-filter/src/models.rs +++ b/crates/thresh-filter/src/models.rs @@ -6,4 +6,5 @@ pub mod ca; pub mod coordinated_turn; pub mod ctrv; pub mod cv; +pub mod equinoctial; pub mod kepler_j2; diff --git a/crates/thresh-filter/src/models/equinoctial.rs b/crates/thresh-filter/src/models/equinoctial.rs new file mode 100644 index 0000000..49ae85b --- /dev/null +++ b/crates/thresh-filter/src/models/equinoctial.rs @@ -0,0 +1,525 @@ +//! Equinoctial-element orbital motion model for sigma-point filters +//! (element-space-orbital-filtering design Decisions 3, 5, 6). +//! +//! State vector `[a, h, k, p, q, λ]` — the stored direct/prograde equinoctial +//! set in `thresh_core::orbital::OrbitalElements::Equinoctial` field order +//! (`sma, h, k, p, q, mean_longitude`), metres and radians. The dynamics are +//! the Gauss variational equations of `thresh_core::orbital::gve` (two-body +//! secular `dλ/dt ⊇ √(µ/a³)` plus a J2 perturbation rotated into RSW), +//! sub-stepped with fixed RK4 inside `predict` — the same step convention as +//! the sibling Cartesian `KeplerJ2` model. Nonlinear: use the UKF/CKF +//! sigma-point machinery (or the EKF via the numeric Jacobian). +//! +//! # Unwrapped mean longitude (design Decision 3) +//! +//! The filter-state mean longitude `λ` is **continuous / unwrapped**: it grows +//! monotonically under the secular rate and is **never** reduced modulo 2π in +//! state space. Periodicity is owned entirely by the element→Cartesian +//! conversion (the trigonometric functions inside are 2π-periodic, and +//! `OrbitalState`'s `as_keplerian` reduces `mean_longitude` mod 2π when it +//! forms the anomaly). Because sigma points are generated around an unwrapped +//! mean and their spread is small relative to 2π, they stay contiguous, so +//! plain arithmetic sigma-point means and residuals remain correct with no +//! circular statistics and no UKF/CKF API change. `predict` therefore never +//! wraps `λ`, and the position measurement map (the only observation of `λ`) +//! goes through the periodic conversion. The `wrap_straddling_*` and +//! `long_horizon_*` tests prove this convention has no wrap artifact and does +//! not degrade as `λ` accumulates hundreds of radians (at `λ ~ 1e3 rad` the +//! f64 `sin`/`cos` argument-reduction loss is ~1e-13 rad — documented, +//! negligible for the demonstrated horizons). +//! +//! # Element-space process noise (design Decision 3 / task 3.4) +//! +//! The process noise follows the state-noise-compensation (SNC) framework of +//! N. Stacey & S. D'Amico, *"Analytical Process Noise Covariance Modeling +//! for Absolute and Relative Orbits"* (arXiv:2105.06516, Acta Astronautica +//! 2022, DOI 10.1016/j.actaastro.2022.01.020; PDF fetched 2026-07-16 from +//! ). That paper models the unmodeled +//! acceleration `ε` as a zero-mean white Gaussian process with autocovariance +//! `E[ε(t)ε(τ)ᵀ] = Q̃ δ(t−τ)` (their Eq. 3), where `Q̃ ∈ R³ˣ³` is the +//! acceleration power spectral density, and gives the process-noise covariance +//! `Q_k = ∫ Φ(t_k,τ) Γ(τ) Q̃ Γ(τ)ᵀ Φ(t_k,τ)ᵀ dτ` (their Eq. 5) with the +//! process-noise mapping matrix `Γ = ∂ẋ/∂ε` (their Eq. 2). For the equinoctial +//! state `dxE/dt = G(xE)·d + [0,…,0,n]ᵀ`, `Γ` is exactly the Gauss variational +//! input matrix `G`. Over one sub-step, with `Φ ≈ I` and `Γ` frozen at the +//! reference elements, Eq. (5) reduces to the leading term +//! `Q ≈ Γ·(σ²I₃)·Γᵀ·Δt`, using the **inertial** acceleration PSD +//! `Q̃ᴵ = σ_accel²·I₃`. The sibling `KeplerJ2` uses the *same* physical PSD: +//! its continuous-white-noise-acceleration block is the closed-form of the +//! *same* Eq. (5) for the kinematic Cartesian state (the paper's Eq. 11), so +//! both models share one physical acceleration-PSD assumption — the fairness +//! requirement of the coast-gap demonstration (design Decision 5). Because +//! `Q̃ᴵ = σ²I₃` is isotropic, the inertial and RSW mappings coincide +//! (`Γᴵ Q̃ᴵ Γᴵᵀ = Γᴿ Q̃ᴿ Γᴿᵀ`), so the mapping is built from unit **inertial** +//! accelerations (no explicit RSW rotation needed). `Q` is state-independent +//! per `predict` interval (the `MotionModel` trait exposes only `dt`), so `G` +//! is evaluated once at the model's [`EquinoctialModel::reference_elements`] — +//! the standard nominal-trajectory SNC linearization. + +use nalgebra::{DMatrix, DVector, Vector3}; + +use thresh_core::orbital::GravityModel; +use thresh_core::orbital::gve::{ + equinoctial_element_rates, j2_perturbation_acceleration, propagate_equinoctial, +}; +use thresh_core::orbital::{Frame, OrbitalElements, OrbitalState}; +use thresh_core::time::Epoch; + +use crate::numeric::numeric_jacobian; +use crate::traits::MotionModel; + +/// Fixed J2000.0 epoch for the throwaway [`OrbitalState`] built only to reuse +/// the crate's tested element→Cartesian conversions. Those conversions are +/// pure geometry — they read neither the epoch nor the frame tag — so this +/// value never affects any propagated element or mapped position (the same +/// convention as `thresh_core::orbital::gve`). +const CONVERSION_EPOCH_JD: f64 = 2_451_545.0; + +/// Numeric-Jacobian per-column floor scales for the 6D element state +/// `[a, h, k, p, q, λ]`. `a` carries its own ~1e7 m magnitude, so its column +/// step tracks that; the dimensionless eccentricity/inclination elements and +/// the angle take a 1e-3 floor so a near-zero component (circular / equatorial +/// orbit, `λ ≈ 0`) still gets a meaningful finite-difference step. The +/// Jacobian exists only to satisfy [`MotionModel`] / feed an EKF — the +/// sigma-point filters never call it. +const ELEMENT_6D_SCALES: [f64; 6] = [1.0, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3]; + +/// Equinoctial-element orbital motion model over the 6D state `[a, h, k, p, +/// q, λ]` (design Decisions 3 and 6). +/// +/// `gravity` supplies `µ` (two-body secular rate and coefficient scaling) and +/// the J2 term (the perturbing acceleration) — parameterized, never a +/// hardcoded Earth constant, like `KeplerJ2`. `predict` sub-steps the interval +/// with fixed RK4 over the Gauss variational element rates (`max_step_s` +/// ceiling). The mean longitude is unwrapped (see the module docs). +pub struct EquinoctialModel { + /// Central-body gravitational parameters (µ, J2, equatorial radius). + pub gravity: GravityModel, + /// RK4 sub-step ceiling (s); `predict` takes `ceil(dt / max_step_s)` equal + /// sub-steps. Default 10.0 (matching `KeplerJ2`). + pub max_step_s: f64, + /// Isotropic unmodeled-acceleration white-noise PSD `σ_accel` + /// (m/s²·Hz^-½), the same physical quantity as `KeplerJ2::sigma_accel`. + /// Default 1e-3. + pub sigma_accel: f64, + /// Nominal elements at which the process-noise input matrix `G` is + /// evaluated (the SNC nominal-trajectory linearization — see the module + /// docs). Typically the filter's initial estimate. + pub reference_elements: [f64; 6], +} + +impl EquinoctialModel { + /// Create a model for the given central body and nominal (reference) + /// elements, with the defaults `max_step_s = 10.0` s and + /// `sigma_accel = 1e-3` m/s²·Hz^-½. + pub fn new(gravity: GravityModel, reference_elements: [f64; 6]) -> Self { + Self { + gravity, + max_step_s: 10.0, + sigma_accel: 1e-3, + reference_elements, + } + } + + /// The J2-only inertial perturbation closure the GVE consume (design + /// Decision 2): two-body is the analytic secular term inside the GVE and + /// is never part of the perturbation input. + fn j2_perturbation(&self) -> impl Fn(f64, &Vector3, &Vector3) -> Vector3 { + let gravity = self.gravity; + move |_t, r, _v| j2_perturbation_acceleration(r, &gravity) + } + + /// Map the element state to inertial Cartesian **position** (m) — the ECI + /// position measurement map for position-measurement filters. Exactly the + /// position component of the state's existing element→Cartesian conversion + /// (spec: "Measurement mapping matches the conversion"). + pub fn position_measurement(&self, state: &DVector) -> DVector { + equinoctial_position(&unpack(state), self.gravity.mu) + } +} + +/// Unpack the leading six components of `state` as `[a, h, k, p, q, λ]`. +fn unpack(state: &DVector) -> [f64; 6] { + [state[0], state[1], state[2], state[3], state[4], state[5]] +} + +/// Elements `[a, h, k, p, q, λ]` → inertial Cartesian position (m), via the +/// crate's tested element→Cartesian conversion (stack-only `OrbitalState`, no +/// heap element representation). Exposed so the coast-gap demonstration and +/// the UKF measurement closure share one mapping. +/// +/// # Panics +/// +/// Never in practice — the `Equinoctial` variant always converts (only the +/// `Tle` variant can fail, which this never constructs). +pub fn equinoctial_position(elements: &[f64; 6], mu: f64) -> DVector { + let state = OrbitalState { + elements: OrbitalElements::Equinoctial { + sma: elements[0], + h: elements[1], + k: elements[2], + p: elements[3], + q: elements[4], + mean_longitude: elements[5], + }, + mu, + frame: Frame::Teme, + epoch: Epoch::from_jde_utc(CONVERSION_EPOCH_JD), + }; + let (r, _) = state + .as_cartesian() + .expect("equinoctial elements always convert to Cartesian"); + DVector::from_row_slice(&[r.x, r.y, r.z]) +} + +/// Inertial process-noise mapping matrix `Γᴵ = ∂(element rates)/∂(inertial +/// perturbing acceleration)` — the 6×3 Gauss variational input matrix for an +/// acceleration modeled in the inertial frame (Stacey & D'Amico Eq. 2). +/// Column `j` is the element-rate response to a unit inertial acceleration +/// along axis `j`, isolated by subtracting the zero-perturbation base rate +/// (the analytic two-body `dλ/dt = n` secular term the GVE add unconditionally). +fn inertial_input_matrix(elements: &[f64; 6], mu: f64) -> DMatrix { + let zero = |_t: f64, _r: &Vector3, _v: &Vector3| Vector3::zeros(); + let base = equinoctial_element_rates(elements, mu, 0.0, &zero); + let mut g = DMatrix::zeros(6, 3); + for axis in 0..3 { + let unit = move |_t: f64, _r: &Vector3, _v: &Vector3| { + let mut a = Vector3::zeros(); + a[axis] = 1.0; + a + }; + let rate = equinoctial_element_rates(elements, mu, 0.0, &unit); + for row in 0..6 { + g[(row, axis)] = rate[row] - base[row]; + } + } + g +} + +impl MotionModel for EquinoctialModel { + fn state_dim(&self) -> usize { + 6 + } + + fn predict(&self, state: &DVector, dt: f64) -> DVector { + let elements = unpack(state); + let out = propagate_equinoctial( + elements, + self.gravity.mu, + 0.0, + dt, + self.max_step_s, + &self.j2_perturbation(), + ); + DVector::from_row_slice(&out) + } + + fn jacobian(&self, state: &DVector, dt: f64) -> DMatrix { + numeric_jacobian( + |x, step| self.predict(x, step), + state, + dt, + &ELEMENT_6D_SCALES, + ) + } + + fn process_noise(&self, dt: f64) -> DMatrix { + // Eq. (5) of Stacey & D'Amico with Φ ≈ I over the interval and the + // isotropic inertial acceleration PSD Q̃ᴵ = σ_accel²·I₃ (their Eq. 3): + // Q ≈ Γᴵ·(σ²I₃)·Γᴵᵀ·Δt = σ²·(Γᴵ Γᴵᵀ)·Δt. + // Symmetric positive-semidefinite (Gram form; rank ≤ 3 — added to the + // full-rank predicted covariance, so the sum stays PD). Γᴵ is frozen + // at the reference elements (module docs). This is the same physical + // PSD KeplerJ2 assumes (its Q is Eq. 5's closed form in Cartesian). + let g = inertial_input_matrix(&self.reference_elements, self.gravity.mu); + let scale = self.sigma_accel * self.sigma_accel * dt; + (&g * g.transpose()) * scale + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::ukf::{UkfParams, UnscentedKalmanFilter}; + use std::f64::consts::TAU; + use thresh_core::orbital::{j2_acceleration, rk4_step}; + + const EARTH: GravityModel = GravityModel::EARTH_WGS84; + const EARTH_MU: f64 = GravityModel::EARTH_WGS84.mu; + + /// Equinoctial elements `[a, h, k, p, q, λ]` for a Keplerian state, via the + /// crate's tested conversion. + fn elements_from_keplerian(kep: [f64; 6]) -> [f64; 6] { + let [sma, ecc, inc, raan, argp, true_anomaly] = kep; + let state = OrbitalState { + elements: OrbitalElements::Keplerian { + sma, + ecc, + inc, + raan, + argp, + true_anomaly, + }, + mu: EARTH_MU, + frame: Frame::Teme, + epoch: Epoch::from_jde_utc(CONVERSION_EPOCH_JD), + }; + let (a, h, k, p, q, l) = state.as_equinoctial().unwrap(); + [a, h, k, p, q, l] + } + + /// A generic inclined, mildly eccentric LEO used across the tests. + fn leo_elements() -> [f64; 6] { + elements_from_keplerian([7_000_000.0, 0.01, 0.9, 1.2, 0.4, 0.0]) + } + + /// Inertial `(r, v)` of an element state. + fn elements_to_state(elements: &[f64; 6]) -> (Vector3, Vector3) { + OrbitalState { + elements: OrbitalElements::Equinoctial { + sma: elements[0], + h: elements[1], + k: elements[2], + p: elements[3], + q: elements[4], + mean_longitude: elements[5], + }, + mu: EARTH_MU, + frame: Frame::Teme, + epoch: Epoch::from_jde_utc(CONVERSION_EPOCH_JD), + } + .as_cartesian() + .unwrap() + } + + /// Cartesian two-body+J2 reference propagator (the independent formulation + /// the GVE path is validated against): sub-stepped RK4 over the shared + /// `j2_acceleration` composite. Returns the endpoint position (m). + fn propagate_cartesian( + r0: Vector3, + v0: Vector3, + dt: f64, + max_step_s: f64, + ) -> Vector3 { + let steps = (dt / max_step_s).ceil().max(1.0) as usize; + let sub = dt / steps as f64; + let accel = |p: &Vector3, _v: &Vector3| j2_acceleration(p, &EARTH); + let (mut r, mut v) = (r0, v0); + for _ in 0..steps { + let (rr, vv) = rk4_step(&r, &v, sub, accel); + r = rr; + v = vv; + } + r + } + + fn assert_symmetric_positive_definite(p: &DMatrix, label: &str) { + let asym = (p - p.transpose()).amax(); + assert!(asym < 1e-6 * p.amax(), "{label}: asymmetry {asym:e}"); + assert!( + p.clone().cholesky().is_some(), + "{label}: covariance is not positive definite" + ); + } + + /// PD diagonal covariance: (1 km)² on `a`, (5e-5)² on the dimensionless + /// elements, `sigma_lambda²` on `λ`. + fn element_covariance(sigma_lambda: f64) -> DMatrix { + DMatrix::from_diagonal(&DVector::from_row_slice(&[ + 1.0e6, + 2.5e-9, + 2.5e-9, + 2.5e-9, + 2.5e-9, + sigma_lambda * sigma_lambda, + ])) + } + + // ── task 3.3: measurement mapping (spec: "Measurement mapping matches the + // conversion") ─────────────────────────────────────────────────────── + + #[test] + fn measurement_mapping_matches_conversion_bitwise() { + let elements = leo_elements(); + let model = EquinoctialModel::new(EARTH, elements); + let x = DVector::from_row_slice(&elements); + let mapped = model.position_measurement(&x); + + // The exact element→Cartesian conversion this must equal. + let (r, _) = elements_to_state(&elements); + for i in 0..3 { + assert_eq!( + mapped[i].to_bits(), + r[i].to_bits(), + "measurement component {i} is not bitwise the conversion" + ); + } + // The free function and the method agree. + let free = equinoctial_position(&elements, EARTH_MU); + assert_eq!(free, mapped); + } + + // ── task 3.4: process noise is the input-matrix-mapped acceleration PSD ── + + #[test] + fn process_noise_is_symmetric_psd_and_scales_with_psd_and_dt() { + let elements = leo_elements(); + let mut model = EquinoctialModel::new(EARTH, elements); + model.sigma_accel = 1e-4; + let q1 = model.process_noise(10.0); + + // Symmetric; positive-semidefinite (Gram form Γ Γᵀ). + assert!((&q1 - q1.transpose()).amax() < 1e-30 * q1.amax().max(1.0)); + let min_eig = q1 + .clone() + .symmetric_eigen() + .eigenvalues + .iter() + .copied() + .fold(f64::INFINITY, f64::min); + assert!( + min_eig > -1e-9 * q1.amax(), + "Q not PSD, min eig {min_eig:e}" + ); + + // Linear in dt (Φ ≈ I leading term) and quadratic in σ_accel. + let q2 = model.process_noise(20.0); + assert!((&q2 - &q1 * 2.0).amax() < 1e-12 * q1.amax()); + model.sigma_accel = 2e-4; // ×2 → Q ×4 + let q4 = model.process_noise(10.0); + assert!((&q4 - &q1 * 4.0).amax() < 1e-9 * q1.amax()); + + // Rank ≤ 3 (three acceleration inputs) but added to a full-rank + // predicted covariance keeps that sum PD — proven in the sigma-point + // test below. + } + + // ── task 3.1: sigma-point prediction (spec: "Sigma-point prediction + // through the element dynamics") ─────────────────────────────────────── + + #[test] + fn sigma_point_prediction_matches_direct_and_is_pd() { + let elements = leo_elements(); + let model = EquinoctialModel::new(EARTH, elements); + let x0 = DVector::from_row_slice(&elements); + let p0 = element_covariance(1e-4); + let dt = 600.0; + + // Direct propagation of the prior mean. + let reference = model.predict(&x0, dt); + + let mut ukf = UnscentedKalmanFilter::new(x0, p0, UkfParams::default()); + ukf.predict(&model, dt); + + // Sigma-point mean matches direct propagation up to the (tiny) + // nonlinearity across the sigma spread. MEASURED (cargo test): amax + // gap ≈ 2.0e-3 (dominated by the a-component in metres over a 600 s + // J2 arc); the dimensionless element components agree to ~1e-12. + // Tolerance 1e-2. + let gap = (&ukf.x - &reference).amax(); + assert!(gap < 1e-2, "sigma-point mean vs direct: amax gap {gap:e}"); + + // Predicted covariance is symmetric positive definite (the rank-≤3 + // process noise added to the full-rank sigma-point moment). + assert_symmetric_positive_definite(&ukf.p, "equinoctial UKF predict"); + } + + // ── task 3.2: wrap-straddle (spec: "Wrap-straddling sigma points produce + // no artifact") ──────────────────────────────────────────────────────── + + #[test] + fn wrap_straddling_sigma_points_produce_no_artifact() { + // Mean λ sits just below 2π; a wide σ_λ with α = 1 spreads the sigma + // points across the 2π boundary. + let mut elements = leo_elements(); + elements[5] = TAU - 0.01; + let model = EquinoctialModel::new(EARTH, elements); + let params = UkfParams { + alpha: 1.0, + beta: 2.0, + kappa: 0.0, + }; + let p0 = element_covariance(0.03); // √6·0.03 ≈ 0.073 rad spread > 0.01 + let dt = 400.0; + + // Run A straddles 2π; run B is the same physical state shifted down by + // exactly one revolution (λ near 0⁻). The dynamics are 2π-periodic in + // λ, so the two runs must agree on the five slow elements and the + // whole covariance, differing only by the constant 2π offset in the λ + // mean — the hallmark of a wrap-artifact-free unwrapped convention. + let mut ukf_a = + UnscentedKalmanFilter::new(DVector::from_row_slice(&elements), p0.clone(), params); + ukf_a.predict(&model, dt); + + let mut elements_b = elements; + elements_b[5] -= TAU; + let mut ukf_b = + UnscentedKalmanFilter::new(DVector::from_row_slice(&elements_b), p0, params); + ukf_b.predict(&model, dt); + + // Five slow elements agree (relative, since `a` is ~1e7). + for i in 0..5 { + let rel = (ukf_a.x[i] - ukf_b.x[i]).abs() / (1.0 + ukf_a.x[i].abs()); + assert!( + rel < 1e-9, + "element {i} differs across the wrap: rel {rel:e}" + ); + } + // λ differs by exactly one revolution (no wrap artifact in the mean). + let lambda_gap = (ukf_a.x[5] - ukf_b.x[5]) - TAU; + assert!( + lambda_gap.abs() < 1e-6, + "λ mean shift {} vs 2π", + ukf_a.x[5] - ukf_b.x[5] + ); + // The covariance is identical (no wrap artifact in the second moment). + let cov_gap = (&ukf_a.p - &ukf_b.p).amax(); + assert!( + cov_gap < 1e-9 * ukf_a.p.amax(), + "covariance differs across the wrap by {cov_gap:e}" + ); + } + + // ── task 3.2: long-horizon accumulation (spec: "Long-horizon accumulation + // stays accurate") ───────────────────────────────────────────────────── + + #[test] + fn long_horizon_lambda_accumulation_stays_accurate() { + let elements = leo_elements(); + let (r0, v0) = elements_to_state(&elements); + let mut model = EquinoctialModel::new(EARTH, elements); + // 5 s steps in both formulations so the residual reflects the + // formulation agreement, not either integrator's coarse-step + // truncation. + model.max_step_s = 5.0; + + // ~50 revolutions: λ accumulates ~50·2π ≈ 314 rad, well into the + // "hundreds of radians" regime. + let period = TAU * (7_000_000_f64.powi(3) / EARTH_MU).sqrt(); + let arc = 50.0 * period; + + let el_end = model.predict(&DVector::from_row_slice(&elements), arc); + // λ is unwrapped: it is NOT reduced mod 2π in state space. + assert!( + el_end[5] > 300.0, + "λ must accumulate unwrapped, got {}", + el_end[5] + ); + + // The converted Cartesian position still agrees with the independent + // Cartesian two-body+J2 path at the cross-formulation tolerance. + let r_gve = model.position_measurement(&el_end); + let r_gve = Vector3::new(r_gve[0], r_gve[1], r_gve[2]); + let r_cart = propagate_cartesian(r0, v0, arc, model.max_step_s); + + // MEASURED (cargo test): gap ≈ 0.109 m over 50 revs at 5 s steps + // (≈ 2.81 m at 10 s steps — the ~26× shrink from halving the step + // confirms the residual is ordinary RK4 truncation, dominated by the + // Cartesian reference's non-analytic two-body integration, NOT a λ + // artifact: the sin/cos argument-reduction loss at λ ~ 3e2 rad is + // ~1e-14 rad, and nothing blows up as λ crosses hundreds of radians). + // Tolerance 0.5 m (a wrap/precision artifact would drift kilometres). + let gap = (r_gve - r_cart).norm(); + assert!(gap < 0.5, "long-horizon GVE vs Cartesian gap = {gap:.4e} m"); + } +} diff --git a/openspec/changes/element-space-orbital-filtering/design.md b/openspec/changes/element-space-orbital-filtering/design.md index bdaa1ce..ca1af7c 100644 --- a/openspec/changes/element-space-orbital-filtering/design.md +++ b/openspec/changes/element-space-orbital-filtering/design.md @@ -63,6 +63,13 @@ Every step additive and independently revertible; nothing touches existing model ## Open Questions -- **Which authority's GVE form ships** (direct-set from DSST/Cefola lineage vs Walker-set internally with boundary conversions) — resolved at implementation by what is actually fetchable; recorded per Decision 1. -- **Process noise in element space**: a diagonal Q on elements is not physically meaningful the way acceleration-driven Q is in Cartesian; options are Van-Loan-style Q from an RSW acceleration PSD mapped through the GVE input matrix, or a small tuned diagonal for the demonstration. Leaning: map an RSW acceleration PSD through the GVE B-matrix (honest, and reuses the demonstration's fairness argument — both filters then share the same physical Q assumption); decide at implementation with the fairness requirement in mind and record. -- **Demonstration gap sweep values** (LEO minutes vs fractions of a period) — pick at implementation for the cleanest recorded separation. +- **Which authority's GVE form ships** (direct-set from DSST/Cefola lineage vs Walker-set internally with boundary conversions) — resolved at implementation by what is actually fetchable; recorded per Decision 1. **Resolved (2026-07-16): the direct-set route.** The GVE for the exact stored set `(a, h, k, p, q, λ)` were fetched in usable closed form from Appendix A of Stacey & D'Amico, "Analytical Process Noise Covariance Modeling for Absolute and Relative Orbits" (arXiv:2105.06516; PDF fetched 2026-07-16), which states the equinoctial Gauss variational equations `dxE/dt = G(xE)·d + [0,…,0,n]ᵀ` for `xE = (a, f=e·cos(ω+Ω), g=e·sin(ω+Ω), h=tan(i/2)cosΩ, k=tan(i/2)sinΩ, λ)` — identical to the stored set under the renaming `f↔k, g↔h, h↔q, k↔p`, so **no Walker/modified-equinoctial boundary conversion was needed** (the recorded fallback was not taken). The appendix cites Battin (ref. 32) and Roth (ref. 33) and flags a sign error in Roth's `H̄`; the transcribed form is the appendix's corrected one. Full provenance, the naming map, and per-coefficient source quotes live in the `gve.rs` module docs; the cross-formulation equivalence suite (Decision 4) validates the transcription independently (all three regimes pass at measured sub-decimetre tolerances). +- **Process noise in element space**: a diagonal Q on elements is not physically meaningful the way acceleration-driven Q is in Cartesian; options are Van-Loan-style Q from an RSW acceleration PSD mapped through the GVE input matrix, or a small tuned diagonal for the demonstration. Leaning: map an RSW acceleration PSD through the GVE B-matrix (honest, and reuses the demonstration's fairness argument — both filters then share the same physical Q assumption); decide at implementation with the fairness requirement in mind and record. **Resolved (2026-07-16): acceleration-PSD mapped through the GVE input matrix (the tuned diagonal was rejected — it does not encode the same physical acceleration PSD as the Cartesian sibling, which would break the demonstration's fairness).** `EquinoctialModel::process_noise` uses the state-noise-compensation framework of Stacey & D'Amico (arXiv:2105.06516; PDF re-fetched this session): model the unmodeled acceleration `ε` as zero-mean white Gaussian with autocovariance `E[εεᵀ]=Q̃ δ(t−τ)` (their Eq. 3), giving `Q_k = ∫ Φ Γ Q̃ Γᵀ Φᵀ dτ` (Eq. 5) with the process-noise mapping matrix `Γ = ∂ẋ/∂ε` (Eq. 2). For the equinoctial state `dxE/dt = G(xE)d + [0,…,0,n]ᵀ`, `Γ` **is** the GVE input matrix `G`; over one sub-step with `Φ ≈ I` and `Γ` frozen at the reference elements, this reduces to the leading term `Q ≈ σ_accel²·(G Gᵀ)·Δt` with the isotropic inertial PSD `Q̃ᴵ = σ_accel²·I₃`. `G` is built numerically from `equinoctial_element_rates` (unit inertial accelerations minus the two-body base rate — isotropy makes the inertial and RSW mappings coincide, so no explicit RSW rotation is needed). Crucially this is the **same** physical PSD `σ_accel²` the Cartesian `KeplerJ2` assumes: its continuous-white-noise-acceleration block is the closed form of the *same* Eq. 5 for the kinematic Cartesian state (the paper's Eq. 11). Both demonstration filters therefore share one physical acceleration-PSD assumption (`σ_accel = 1e-6` in the demonstration), satisfying the fairness requirement. `Q` is evaluated at the model's `reference_elements` because the `MotionModel` trait exposes only `dt` (the standard nominal-trajectory SNC linearization); `Q` is symmetric PSD (rank ≤ 3, added to the full-rank predicted covariance, keeping the sum PD). Full provenance and the per-coefficient mapping live in the `equinoctial.rs` module docs. +- **Demonstration gap sweep values** (LEO minutes vs fractions of a period) — pick at implementation for the cleanest recorded separation. **Resolved (2026-07-16): fractions/multiples of the LEO period, swept 0 → ~4 revolutions at half-revolution spacing.** The demonstration (`thresh-eval/tests/orbital_coast_gap.rs`) tracks a 7000 km / 51.6° LEO (period ≈ 5560 s ≈ 92.7 steps of 60 s), warms up 5 measurements (σ_z = 200 m, initial σ_r = 3000 m, σ_v = 60 m/s → the large-uncertainty coast regime Decision 5 targets), then coasts predict-only through gaps at steps `[0, 46, 93, 139, 185, 232, 278, 325, 371]`. **Recorded sweep** (RUNS = 50, seed = 7, dof 3, band ≈ [2.360, 3.716]): Cartesian-EKF / equinoctial-UKF position ANEES = `0.00 rev` 2.983/2.983, `0.47` 2.906/2.577, `0.96` 3.239/2.912, `1.43` 3.012/2.853, `1.90` 3.050/2.837, **`2.39` 4.757/2.853**, `2.86` 3.107/2.717, **`3.35` 4.370/2.890**, `3.82` 3.309/2.628. The Cartesian along-track banana pushes the EKF out of band once per revolution (its major axis rotating with the orbit — hence the ~3.0↔4.5 oscillation), while the equinoctial UKF stays inside the band at every gap. **Crossover** (spec exit criterion) at **2.39 rev** (232 min into coast, 13 920 s): Cartesian ANEES 4.757 — out of band by margin +1.041 above the upper bound — while the equinoctial ANEES 2.853 is inside (margin +0.494 above lower, −0.863 below upper); a second crossover at 3.35 rev (4.370 vs 2.890). Reruns are bitwise identical (asserted). **Implementation-time divergence recorded:** the coast-phase comparison Decision 5 specifies is isolated by establishing one shared consistent posterior at the warmup end (via the Cartesian EKF that is also the baseline) and initializing the equinoctial UKF from it by an unscented transform, rather than having the equinoctial UKF process the warmup ECI-position updates itself. Position measurements are linear in Cartesian but nonlinear in equinoctial elements, and the statistically-linearized UKF measurement update over the nonlinear element→position map is mildly but persistently underconfident (verified: element-space NEES degrades ≈ 1.2× per update, independent of the sigma-point spread α and of state scaling — a genuine statistical-linearization effect, not numerical); that measurement-update trade-off is orthogonal to Decision 5's coast-propagation claim, so the demonstration keeps the comparison to the predict-only coast, where the diagnostics confirm the equinoctial UKF is consistent (element-space NEES ≈ dof at gap start and through the coast). + +## Implementation-Time Divergences (task 5.2) + +- **The demonstration's equinoctial filter coasts from the shared warmup posterior rather than processing warmup measurements itself** (recorded also at the test and the resolved 4.1 open question): ECI-position measurements are linear in Cartesian but nonlinear in elements, and the statistically-linearized UKF measurement update over the element→position map is *genuinely* underconfident (measured ≈ 1.2× element-space NEES growth per update, independent of sigma-point spread and state scaling). Decision 5's claim is specifically about coast-phase covariance propagation, so both filters share one warmup posterior (Cartesian EKF, converted to elements by unscented transform) and diverge only through the gap — the fair isolation of the claim under test. Element-space *measurement-update* consistency is a distinct problem left to the tracker-consumption follow-up. +- **Process noise resolved as the acceleration-PSD-through-GVE-input-matrix route** (Stacey & D'Amico Eq. 5, Γ built numerically from unit inertial accelerations) — the same isotropic acceleration PSD the Cartesian model's CWNA block encodes, so demonstration fairness is by construction; the tuned-diagonal alternative was rejected for breaking exactly that. +- **The GVE source doubles as the process-noise source**: Stacey & D'Amico (arXiv:2105.06516) supplied both the direct-set GVE (Appendix A, with the corrected Roth H̄ sign) and the SNC input-matrix construction — one fetched, cited authority for the change's two central transcriptions; the cross-formulation equivalence suite validates the GVE independently of that paper. +- **A bonus structural test beyond the spec**: circular-orbit reductions of the transcribed coefficients against the classical Gauss form (`circular_orbit_reductions_match_classical_gauss`), catching coefficient errors the zero-perturbation scenario cannot see. diff --git a/openspec/changes/element-space-orbital-filtering/tasks.md b/openspec/changes/element-space-orbital-filtering/tasks.md index d239bdd..c383665 100644 --- a/openspec/changes/element-space-orbital-filtering/tasks.md +++ b/openspec/changes/element-space-orbital-filtering/tasks.md @@ -4,29 +4,29 @@ ## 1. GVE — `thresh-core/src/orbital/gve.rs` (design Decisions 1–2) -- [ ] 1.1 Fetch the authoritative element-rate equations for the direct equinoctial set `(a, h, k, p, q, λ)` (DSST/Cefola lineage or Vallado's equinoctial VOP; cite what is actually fetched); resolve design Decision 1's fallback — if only Walker-set `(p, f, g, h, k, L)` equations are fetchable in usable form, implement internally in that set with exact boundary conversions — and RECORD the route taken in design.md's Open Questions. -- [ ] 1.2 Implement the element rates as per-element phase helpers (complexity ≤ 15 each) taking the RSW perturbation components; the two-body secular term `dλ/dt ⊇ √(μ/a³)` analytic; RSW basis construction (R̂ = r̂, Ŵ ∝ r×v, Ŝ = Ŵ×R̂) with orthonormality/right-handedness test (spec: "RSW rotation is orthonormal and right-handed"); the perturbing-acceleration seam is a time-aware inertial closure rotated internally (design Decision 2 — J2 enters as `j2_acceleration` alone, never the two-body composite). Tests: zero perturbation moves only λ at exactly the mean motion (spec: "Unperturbed motion moves only the mean longitude"); fetched published spot values where available (spec: "Published spot values reproduced"); near-singular domain e = 1e-8, i = 1e-8 finite (spec: "Near-circular near-equatorial evaluation is finite") with singular denominators identified at the code. -- [ ] 1.3 Element-space propagation: sub-stepped fixed RK4 over the element rates (`max_step_s` configurable, the KeplerJ2 pattern). Tests: bitwise-deterministic repeated propagation (spec: "Deterministic element propagation"); step-halving convergence at the integrator's order on a J2 LEO arc (spec: "Convergence under step halving"). +- [x] 1.1 Fetch the authoritative element-rate equations for the direct equinoctial set `(a, h, k, p, q, λ)` (DSST/Cefola lineage or Vallado's equinoctial VOP; cite what is actually fetched); resolve design Decision 1's fallback — if only Walker-set `(p, f, g, h, k, L)` equations are fetchable in usable form, implement internally in that set with exact boundary conversions — and RECORD the route taken in design.md's Open Questions. +- [x] 1.2 Implement the element rates as per-element phase helpers (complexity ≤ 15 each) taking the RSW perturbation components; the two-body secular term `dλ/dt ⊇ √(μ/a³)` analytic; RSW basis construction (R̂ = r̂, Ŵ ∝ r×v, Ŝ = Ŵ×R̂) with orthonormality/right-handedness test (spec: "RSW rotation is orthonormal and right-handed"); the perturbing-acceleration seam is a time-aware inertial closure rotated internally (design Decision 2 — J2 enters as `j2_acceleration` alone, never the two-body composite). Tests: zero perturbation moves only λ at exactly the mean motion (spec: "Unperturbed motion moves only the mean longitude"); fetched published spot values where available (spec: "Published spot values reproduced"); near-singular domain e = 1e-8, i = 1e-8 finite (spec: "Near-circular near-equatorial evaluation is finite") with singular denominators identified at the code. +- [x] 1.3 Element-space propagation: sub-stepped fixed RK4 over the element rates (`max_step_s` configurable, the KeplerJ2 pattern). Tests: bitwise-deterministic repeated propagation (spec: "Deterministic element propagation"); step-halving convergence at the integrator's order on a J2 LEO arc (spec: "Convergence under step halving"). ## 2. Cross-formulation equivalence (design Decision 4) -- [ ] 2.1 Equivalence suite: identical J2 orbits through GVE-in-elements and the existing Cartesian two-body+J2 path, converted and compared at the endpoints, both directions (element-born and Cartesian-born), three regimes — near-circular LEO, MEO, eccentric e ∈ [0.3, 0.7] through multiple perigee passages — over multi-revolution arcs; tolerances measured at the chosen steps and documented at each test (specs: "LEO equivalence both directions", "Eccentric-orbit equivalence through perigee"). No python generator (design Decision 4's deliberate record). +- [x] 2.1 Equivalence suite: identical J2 orbits through GVE-in-elements and the existing Cartesian two-body+J2 path, converted and compared at the endpoints, both directions (element-born and Cartesian-born), three regimes — near-circular LEO, MEO, eccentric e ∈ [0.3, 0.7] through multiple perigee passages — over multi-revolution arcs; tolerances measured at the chosen steps and documented at each test (specs: "LEO equivalence both directions", "Eccentric-orbit equivalence through perigee"). No python generator (design Decision 4's deliberate record). ## 3. Filter model — `thresh-filter/src/models/equinoctial.rs` (design Decision 3) -- [ ] 3.1 `MotionModel` impl over the 6D element state (state order = the stored `OrbitalElements::Equinoctial` field order), `mu` + `max_step_s` parameterized; UKF sigma-point prediction test: predicted mean matches direct propagation within sigma-point tolerance, covariance symmetric PD (spec: "Sigma-point prediction through the element dynamics"). -- [ ] 3.2 Unwrapped-λ convention (design Decision 3): documented on the model; wrap-straddle test — mean λ just below 2π·k with sigma spread crossing, predicted mean/covariance continuous and equal (to tolerance) to a 2π-shifted run (spec: "Wrap-straddling sigma points produce no artifact"); long-horizon test — λ accumulating hundreds of radians still matches the Cartesian path within the cross-formulation tolerance, with the f64 sin/cos precision note documented (spec: "Long-horizon accumulation stays accurate"). -- [ ] 3.3 Measurement mapping elements → inertial Cartesian position, exactly equal to the existing conversion (spec: "Measurement mapping matches the conversion"). -- [ ] 3.4 Resolve the element-space process-noise open question (design: RSW acceleration PSD mapped through the GVE input matrix vs tuned diagonal) with the demonstration-fairness requirement in mind; implement, and RECORD the decision in design.md. +- [x] 3.1 `MotionModel` impl over the 6D element state (state order = the stored `OrbitalElements::Equinoctial` field order), `mu` + `max_step_s` parameterized; UKF sigma-point prediction test: predicted mean matches direct propagation within sigma-point tolerance, covariance symmetric PD (spec: "Sigma-point prediction through the element dynamics"). +- [x] 3.2 Unwrapped-λ convention (design Decision 3): documented on the model; wrap-straddle test — mean λ just below 2π·k with sigma spread crossing, predicted mean/covariance continuous and equal (to tolerance) to a 2π-shifted run (spec: "Wrap-straddling sigma points produce no artifact"); long-horizon test — λ accumulating hundreds of radians still matches the Cartesian path within the cross-formulation tolerance, with the f64 sin/cos precision note documented (spec: "Long-horizon accumulation stays accurate"). +- [x] 3.3 Measurement mapping elements → inertial Cartesian position, exactly equal to the existing conversion (spec: "Measurement mapping matches the conversion"). +- [x] 3.4 Resolve the element-space process-noise open question (design: RSW acceleration PSD mapped through the GVE input matrix vs tuned diagonal) with the demonstration-fairness requirement in mind; implement, and RECORD the decision in design.md. ## 4. Demonstration + invariance (design Decision 5) -- [ ] 4.1 The Monte-Carlo coast-gap experiment in thresh-eval (dev-dep on thresh-filter already exists): seeded truth arcs (Cartesian two-body+J2), noisy ECI position measurements, warmup tracking, coast gap of duration T, ANEES at gap end over N runs — UKF-over-equinoctial vs EKF-over-KeplerJ2, both sharing the physical process-noise assumption from 3.4; sweep T; resolve the sweep-values open question for the cleanest separation and RECORD it in design.md. -- [ ] 4.2 Assertions: at least one recorded gap duration where the Cartesian baseline's ANEES is outside the two-sided 95% band while the equinoctial filter's is inside, margins documented (spec: "Element filter outlasts the Cartesian filter through coast"); bitwise-identical reruns (spec: "Demonstration is deterministic"); full sweep values recorded at the test. -- [ ] 4.3 Exit criterion (numeric, falsifiable): the cross-formulation equivalence suite passes at its documented tolerances in all three regimes and both directions; the wrap-straddle and long-horizon λ tests hold; and the demonstration sweep records the crossover — Cartesian ANEES out of band, equinoctial in band — at a stated gap duration. -- [ ] 4.4 Benchmark invariance: the four calibrated scenarios digit-for-digit identical before vs after (spec: "Calibrated benchmarks are bitwise unchanged"); record the comparison in the PR description. +- [x] 4.1 The Monte-Carlo coast-gap experiment in thresh-eval (dev-dep on thresh-filter already exists): seeded truth arcs (Cartesian two-body+J2), noisy ECI position measurements, warmup tracking, coast gap of duration T, ANEES at gap end over N runs — UKF-over-equinoctial vs EKF-over-KeplerJ2, both sharing the physical process-noise assumption from 3.4; sweep T; resolve the sweep-values open question for the cleanest separation and RECORD it in design.md. +- [x] 4.2 Assertions: at least one recorded gap duration where the Cartesian baseline's ANEES is outside the two-sided 95% band while the equinoctial filter's is inside, margins documented (spec: "Element filter outlasts the Cartesian filter through coast"); bitwise-identical reruns (spec: "Demonstration is deterministic"); full sweep values recorded at the test. +- [x] 4.3 Exit criterion (numeric, falsifiable): the cross-formulation equivalence suite passes at its documented tolerances in all three regimes and both directions; the wrap-straddle and long-horizon λ tests hold; and the demonstration sweep records the crossover — Cartesian ANEES out of band, equinoctial in band — at a stated gap duration. +- [x] 4.4 Benchmark invariance: the four calibrated scenarios digit-for-digit identical before vs after (spec: "Calibrated benchmarks are bitwise unchanged"); record the comparison in the PR description. ## 5. Wrap-up -- [ ] 5.1 Full gates: `cargo test --workspace`, `cargo clippy --workspace --all-targets -- -D warnings`, `cargo fmt --all -- --check`, rustdoc `-Dwarnings`, `openspec validate --all --strict --no-interactive`; complexity spot-check on the element-rate helpers and the sweep harness. Also `cargo test -p thresh-data --features adsb` (the feature-gated-manifest lesson from #145). -- [ ] 5.2 Update proposal.md/design.md with implementation-time divergences and the resolutions of the three design Open Questions (GVE source/set route, element-space Q, sweep values). +- [x] 5.1 Full gates: `cargo test --workspace`, `cargo clippy --workspace --all-targets -- -D warnings`, `cargo fmt --all -- --check`, rustdoc `-Dwarnings`, `openspec validate --all --strict --no-interactive`; complexity spot-check on the element-rate helpers and the sweep harness. Also `cargo test -p thresh-data --features adsb` (the feature-gated-manifest lesson from #145). +- [x] 5.2 Update proposal.md/design.md with implementation-time divergences and the resolutions of the three design Open Questions (GVE source/set route, element-space Q, sweep values). From 87b26a2eb5dd36fdcbbf92c4676b432f90777ce6 Mon Sep 17 00:00:00 2001 From: Evan Montgomery-Recht Date: Thu, 16 Jul 2026 22:59:02 -0400 Subject: [PATCH 2/2] fix(review): backward-dt substeps, Cartesian-born equivalence, full-sweep asserts, phase-tracking Q MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - gve.rs: substep count uses |dt| (backward propagation splits correctly) and propagate_equinoctial validates max_step_s (finite, > 0) - gve.rs: genuinely Cartesian-born equivalence direction added — (r0, v0) from keplerian_to_cartesian, GVE elements born FROM that state via cartesian_to_keplerian, so no shared elements-first initialization can cancel a conversion defect; measured gap 0.138 m over 3 revs is initial-condition conditioning (near-circular round trip ~mm delta-a drifting 3*pi per rev), documented, tolerance 1.0 m vs km-scale for a real defect - orbital_coast_gap.rs: the full stated result is now asserted — the equinoctial UKF in-band at ALL nine gaps and both recorded Cartesian excursions (checkpoints 5 and 7), not just one crossover - EquinoctialModel: re_anchored(state) helper (Clone+Copy derived); the SNC input matrix Gamma depends on orbital phase, so the demo re-anchors per coast step and the model documents the small-interval approximation and the re-anchoring contract for long arcs; demo numbers unchanged at recorded precision Co-Authored-By: Claude Fable 5 --- crates/thresh-core/src/orbital/gve.rs | 69 +++++++++++++++++-- crates/thresh-eval/tests/orbital_coast_gap.rs | 28 +++++++- .../thresh-filter/src/models/equinoctial.rs | 15 ++++ 3 files changed, 106 insertions(+), 6 deletions(-) diff --git a/crates/thresh-core/src/orbital/gve.rs b/crates/thresh-core/src/orbital/gve.rs index 98caf8c..94a8d99 100644 --- a/crates/thresh-core/src/orbital/gve.rs +++ b/crates/thresh-core/src/orbital/gve.rs @@ -347,12 +347,14 @@ where ] } -/// Number of equal RK4 sub-steps covering `dt`: `ceil(dt / max_step_s)`, at -/// least 1 (so `dt = 0` degenerates to a single identity step). Mirrors the -/// `KeplerJ2` Cartesian model's sub-stepping so the two formulations share a -/// step convention. +/// Number of equal RK4 sub-steps covering `dt`: `ceil(|dt| / max_step_s)`, +/// at least 1 (so `dt = 0` degenerates to a single identity step, and a +/// negative `dt` — backward propagation — is split into the same +/// ceiling-sized substeps instead of one large step). Mirrors the +/// `KeplerJ2` Cartesian model's sub-stepping so the two formulations share +/// a step convention. fn substep_count(dt: f64, max_step_s: f64) -> usize { - (dt / max_step_s).ceil().max(1.0) as usize + (dt.abs() / max_step_s).ceil().max(1.0) as usize } /// `x + a·y` element-wise for the 6-vector of elements (RK4 stage state). @@ -401,6 +403,10 @@ pub fn propagate_equinoctial( where F: Fn(f64, &Vector3, &Vector3) -> Vector3, { + assert!( + max_step_s.is_finite() && max_step_s > 0.0, + "max_step_s must be finite and > 0, got {max_step_s}" + ); let steps = substep_count(dt, max_step_s); let sub_dt = dt / steps as f64; let mut x = elements; @@ -707,6 +713,37 @@ mod tests { (r_gve - r_cart).norm() } + /// The genuinely Cartesian-born direction: `(r0, v0)` come straight from + /// `keplerian_to_cartesian` (a third conversion path), and the GVE side + /// receives elements converted FROM that Cartesian state — so the two + /// propagations do not share an elements-first initialization, and a + /// defect in the equinoctial→Cartesian conversion cannot cancel. + fn gve_vs_cartesian_gap_cartesian_born(case: &EquivCase) -> f64 { + let [sma, ecc, inc, raan, argp, true_anomaly] = case.kep; + let (r0, v0) = crate::orbital::keplerian_to_cartesian( + sma, + ecc, + inc, + raan, + argp, + true_anomaly, + EARTH_MU, + ); + // Round-trip the CARTESIAN state back through cartesian_to_keplerian + // so the GVE side's elements are born from (r0, v0), not from the + // Keplerian inputs both sides would otherwise share. + let (k_sma, k_ecc, k_inc, k_raan, k_argp, k_nu) = + crate::orbital::cartesian_to_keplerian(&r0, &v0, EARTH_MU); + let el = elements_from_keplerian(k_sma, k_ecc, k_inc, k_raan, k_argp, k_nu, EARTH_MU); + + let el_end = + propagate_equinoctial(el, EARTH_MU, 0.0, case.arc, case.step, &j2_closure(EARTH)); + let r_gve = elements_to_position(&el_end, EARTH_MU); + + let (r_cart, _) = propagate_cartesian(r0, v0, EARTH, case.arc, case.step); + (r_gve - r_cart).norm() + } + #[test] fn leo_equivalence_both_directions() { // Near-circular LEO, ~3 revolutions (period ≈ 5560 s), 5 s steps. @@ -720,6 +757,28 @@ mod tests { // (dominated by the Cartesian RK4 two-body truncation — the GVE // two-body part is analytic). Tolerance 0.1 m. assert!(gap < 0.1, "LEO GVE vs Cartesian gap = {gap:.4e} m"); + + // The genuinely Cartesian-born direction (spec: "LEO equivalence + // both directions"): (r0, v0) from keplerian_to_cartesian, the GVE + // elements born FROM that Cartesian state via cartesian_to_keplerian + // — no shared elements-first initialization, so a conversion defect + // cannot cancel between the two sides. + let gap_cb = gve_vs_cartesian_gap_cartesian_born(&EquivCase { + kep: [7_000_000.0, 0.01, 0.9, 1.2, 0.4, 0.0], + arc: 3.0 * period, + step: 5.0, + }); + // MEASURED: gap ≈ 1.38e-1 m — 50× the element-born case, and that is + // initial-condition conditioning, not propagation error: the + // keplerian↔cartesian round trip at e = 0.01 reproduces the state + // to ~mm (near-circular argp/ν conditioning), and an initial δa + // drifts along-track by ≈ 3π·δa per revolution (5 mm × 3 rev × 3π + // ≈ 0.14 m). A genuine conversion or GVE defect would be km-scale; + // the element-born case above isolates propagation truth at 2.75e-3 m. + assert!( + gap_cb < 1.0, + "LEO Cartesian-born GVE vs Cartesian gap = {gap_cb:.4e} m" + ); } #[test] diff --git a/crates/thresh-eval/tests/orbital_coast_gap.rs b/crates/thresh-eval/tests/orbital_coast_gap.rs index df22dee..4211695 100644 --- a/crates/thresh-eval/tests/orbital_coast_gap.rs +++ b/crates/thresh-eval/tests/orbital_coast_gap.rs @@ -340,7 +340,11 @@ fn coast_run( for c in 0..=max_coast { if c > 0 { ekf.predict(&kepler, DT_STEP); - ukf.predict(&equinoctial, DT_STEP); + // Re-anchor the SNC input matrix at the current mean so Q's + // element-space orientation tracks orbital phase through the + // multi-revolution coast (see EquinoctialModel::re_anchored). + let step_model = equinoctial.re_anchored(&ukf.x); + ukf.predict(&step_model, DT_STEP); } if CHECKPOINTS.contains(&c) { let truth_pos = &truth[WARMUP_STEPS + c].0; @@ -443,6 +447,28 @@ fn coast_gap_element_filter_outlasts_cartesian() { "equinoctial ANEES {ukf_mean} must be inside [{lo}, {hi}]" ); + // The stated result is stronger than one crossover: the equinoctial UKF + // stays inside the band at EVERY swept gap — assert the whole sweep so a + // regression at any other checkpoint cannot hide behind the crossover. + for (i, acc) in equi_accs.iter().enumerate() { + assert_eq!( + acc.verdict(chi2::DEFAULT_ALPHA).unwrap(), + ConsistencyVerdict::Consistent, + "equinoctial ANEES must be in band at checkpoint {i} (gap {} s), got {:?}", + CHECKPOINTS[i] as f64 * DT_STEP, + acc.mean() + ); + } + // Both recorded Cartesian excursions (the once-per-revolution banana + // rotation) are part of the recorded, bitwise-deterministic result. + for &idx in &[5usize, 7usize] { + assert_ne!( + cart_accs[idx].verdict(chi2::DEFAULT_ALPHA).unwrap(), + ConsistencyVerdict::Consistent, + "Cartesian ANEES expected out of band at checkpoint {idx}" + ); + } + // Spec "Demonstration is deterministic": a second seeded sweep reproduces // every accumulated ANEES bit-for-bit. let (cart2, equi2) = run_sweep(&truth, ref_elem); diff --git a/crates/thresh-filter/src/models/equinoctial.rs b/crates/thresh-filter/src/models/equinoctial.rs index 49ae85b..14833fd 100644 --- a/crates/thresh-filter/src/models/equinoctial.rs +++ b/crates/thresh-filter/src/models/equinoctial.rs @@ -92,6 +92,7 @@ const ELEMENT_6D_SCALES: [f64; 6] = [1.0, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3]; /// hardcoded Earth constant, like `KeplerJ2`. `predict` sub-steps the interval /// with fixed RK4 over the Gauss variational element rates (`max_step_s` /// ceiling). The mean longitude is unwrapped (see the module docs). +#[derive(Debug, Clone, Copy)] pub struct EquinoctialModel { /// Central-body gravitational parameters (µ, J2, equatorial radius). pub gravity: GravityModel, @@ -121,6 +122,20 @@ impl EquinoctialModel { } } + /// A copy of this model re-anchored at `state` (the current filter + /// mean): the process-noise input matrix Γ depends on orbital phase, so + /// evaluating it at a fixed [`Self::reference_elements`] is a + /// small-interval approximation (the cited SNC model's own framing). + /// Long-arc consumers SHOULD re-anchor per predict step so Q's + /// element-space orientation tracks the orbit. + pub fn re_anchored(&self, state: &DVector) -> Self { + let mut m = *self; + for (slot, value) in m.reference_elements.iter_mut().zip(state.iter()) { + *slot = *value; + } + m + } + /// The J2-only inertial perturbation closure the GVE consume (design /// Decision 2): two-body is the analytic secular term inside the GVE and /// is never part of the perturbation input.