diff --git a/Cargo.lock b/Cargo.lock index 6c09743..36a7744 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -710,6 +710,27 @@ version = "0.1.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7eed2c4702fa172d1ce21078faa7c5203e69f5394d48cc436d25928394a867a2" +[[package]] +name = "differential-equations" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9d59c6f4160bf75916dd69b58f6323171dd22e2e4a68cf7e31b1b2bf54b7433a" +dependencies = [ + "differential-equations-derive", + "simba", +] + +[[package]] +name = "differential-equations-derive" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68a7fe2019075c678918caa15cc0be080f0c4400ae58256aaf31ab7059b4920b" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + [[package]] name = "digest" version = "0.10.7" @@ -1033,6 +1054,114 @@ version = "0.32.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e629b9b98ef3dd8afe6ca2bd0f89306cec16d43d907889945bc5d6687f2f13c7" +[[package]] +name = "glam" +version = "0.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "333928d5eb103c5d4050533cec0384302db6be8ef7d3cebd30ec6a35350353da" + +[[package]] +name = "glam" +version = "0.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3abb554f8ee44336b72d522e0a7fe86a29e09f839a36022fa869a7dfe941a54b" + +[[package]] +name = "glam" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4126c0479ccf7e8664c36a2d719f5f2c140fbb4f9090008098d2c291fa5b3f16" + +[[package]] +name = "glam" +version = "0.17.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e01732b97afd8508eee3333a541b9f7610f454bb818669e66e90f5f57c93a776" + +[[package]] +name = "glam" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "525a3e490ba77b8e326fb67d4b44b4bd2f920f44d4cc73ccec50adc68e3bee34" + +[[package]] +name = "glam" +version = "0.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b8509e6791516e81c1a630d0bd7fbac36d2fa8712a9da8662e716b52d5051ca" + +[[package]] +name = "glam" +version = "0.20.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f43e957e744be03f5801a55472f593d43fabdebf25a4585db250f04d86b1675f" + +[[package]] +name = "glam" +version = "0.21.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "518faa5064866338b013ff9b2350dc318e14cc4fcd6cb8206d7e7c9886c98815" + +[[package]] +name = "glam" +version = "0.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "12f597d56c1bd55a811a1be189459e8fad2bbc272616375602443bdfb37fa774" + +[[package]] +name = "glam" +version = "0.23.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e4afd9ad95555081e109fe1d21f2a30c691b5f0919c67dfa690a2e1eb6bd51c" + +[[package]] +name = "glam" +version = "0.24.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5418c17512bdf42730f9032c74e1ae39afc408745ebb2acf72fbc4691c17945" + +[[package]] +name = "glam" +version = "0.25.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "151665d9be52f9bb40fc7966565d39666f2d1e69233571b71b87791c7e0528b3" + +[[package]] +name = "glam" +version = "0.27.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e05e7e6723e3455f4818c7b26e855439f7546cf617ef669d1adedb8669e5cb9" + +[[package]] +name = "glam" +version = "0.28.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "779ae4bf7e8421cf91c0b3b64e7e8b40b862fba4d393f59150042de7c4965a94" + +[[package]] +name = "glam" +version = "0.29.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8babf46d4c1c9d92deac9f7be466f76dfc4482b6452fc5024b5e8daf6ffeb3ee" + +[[package]] +name = "glam" +version = "0.30.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19fc433e8437a212d1b6f1e68c7824af3aed907da60afa994e7f542d18d12aa9" + +[[package]] +name = "glam" +version = "0.31.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "556f6b2ea90b8d15a74e0e7bb41671c9bdf38cd9f78c284d750b9ce58a2b5be7" + +[[package]] +name = "glam" +version = "0.32.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f70749695b063ecbf6b62949ccccde2e733ec3ecbbd71d467dca4e5c6c97cca0" + [[package]] name = "glob" version = "0.3.3" @@ -1638,11 +1767,29 @@ dependencies = [ [[package]] name = "nalgebra" -version = "0.33.3" +version = "0.34.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9d43ddcacf343185dfd6de2ee786d9e8b1c2301622afab66b6c73baf9882abfd" +checksum = "df76ea0ff5c7e6b88689085804d6132ded0ddb9de5ca5b8aeb9eeadc0508a70a" dependencies = [ "approx", + "glam 0.14.0", + "glam 0.15.2", + "glam 0.16.0", + "glam 0.17.3", + "glam 0.18.0", + "glam 0.19.0", + "glam 0.20.5", + "glam 0.21.3", + "glam 0.22.0", + "glam 0.23.0", + "glam 0.24.2", + "glam 0.25.0", + "glam 0.27.0", + "glam 0.28.0", + "glam 0.29.3", + "glam 0.30.10", + "glam 0.31.1", + "glam 0.32.1", "matrixmultiply", "nalgebra-macros", "num-complex", @@ -1654,9 +1801,9 @@ dependencies = [ [[package]] name = "nalgebra-macros" -version = "0.2.2" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" +checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" dependencies = [ "proc-macro2", "quote", @@ -1829,6 +1976,7 @@ dependencies = [ "approx", "camino", "criterion", + "differential-equations", "directories", "hifitime", "husky-rs", diff --git a/Cargo.toml b/Cargo.toml index deafba9..b883e9d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -31,7 +31,7 @@ hifitime = { version = "4.2.0", default-features = false, features = [ "ut1", "std", ] } -nalgebra = { version = "0.33.2" } +nalgebra = { version = "0.34.2" } ordered-float = { version = "5.0.0", default-features = false } smallvec = { version = "1.14.0", default-features = false } thiserror = { version = "2.0.12", default-features = false } @@ -58,6 +58,7 @@ tokio = { version = "1.44.1", default-features = false, features = [ ] } tokio-stream = { version = "0.1.17", default-features = false } rayon = { version = "1.12.0", default-features = false, optional = true } +differential-equations = { version = "0.6.1", default-features = false } [dev-dependencies] approx = { version = "0.5.1", default-features = false } diff --git a/examples/run_full_iod.rs b/examples/run_full_iod.rs index 6b64b9a..bb4317b 100644 --- a/examples/run_full_iod.rs +++ b/examples/run_full_iod.rs @@ -57,6 +57,8 @@ fn outfit_error_label(err: &OutfitError) -> &'static str { OutfitError::BizarreOrbit => "BizarreOrbit", OutfitError::DifferentialCorrectionDiverged => "DifferentialCorrectionDiverged", OutfitError::DifferentialCorrectionFailed(_) => "DifferentialCorrectionFailed", + OutfitError::EphemerisBodyNotSupported(_) => "EphemerisBodyNotSupported", + OutfitError::NBodyPropagationFailed(_) => "NBodyPropagationFailed", } } diff --git a/examples/run_full_iod_parallel.rs b/examples/run_full_iod_parallel.rs index 5fe8250..9faf6b5 100644 --- a/examples/run_full_iod_parallel.rs +++ b/examples/run_full_iod_parallel.rs @@ -59,6 +59,8 @@ fn outfit_error_label(err: &OutfitError) -> &'static str { OutfitError::BizarreOrbit => "DiffCorBizarreOrbit", OutfitError::DifferentialCorrectionDiverged => "DiffCorDiverged", OutfitError::DifferentialCorrectionFailed(_) => "DiffCorFailed", + OutfitError::EphemerisBodyNotSupported(_) => "EphemerisBodyNotSupported", + OutfitError::NBodyPropagationFailed(_) => "NBodyPropagationFailed", } } diff --git a/src/differential_orbit_correction/diff_cor.rs b/src/differential_orbit_correction/diff_cor.rs index 7c27bc2..98c2afe 100644 --- a/src/differential_orbit_correction/diff_cor.rs +++ b/src/differential_orbit_correction/diff_cor.rs @@ -56,6 +56,7 @@ use crate::{ single_iteration::single_iteration, }, orbit_type::equinoctial_element::EquinoctialLimits, + propagator::PropagatorKind, EquinoctialElements, JPLEphem, OutfitError, }; @@ -159,6 +160,13 @@ pub struct DifferentialCorrectionConfig { /// /// Default: `[true; 6]` (all elements free). pub free_elements: [bool; 6], + + /// Propagator to use for computing predicted observations and partials. + /// + /// - [`PropagatorKind::TwoBody`] (default): analytic Keplerian propagation. + /// - [`PropagatorKind::NBody`]: numerical DOP853 N-body integration with + /// user-specified perturbing bodies. + pub propagator: PropagatorKind, } impl Default for DifferentialCorrectionConfig { @@ -175,6 +183,7 @@ impl Default for DifferentialCorrectionConfig { outlier_rejection_config: OutlierRejectionConfig::default(), orbital_limits: EquinoctialLimits::default(), free_elements: [true; 6], + propagator: PropagatorKind::TwoBody, } } } @@ -300,19 +309,9 @@ pub fn run_differential_correction( cache, jpl, true, + &config.propagator, )?; - println!( - "Corrected elements after iteration {total_newton_iterations}: {:?}", - iter_result.corrected_elements - ); - println!( - "IterResult correction norm: {}, normalised RMS: {}, num active measurements: {}\n\n", - iter_result.correction_norm, - iter_result.normalised_rms, - iter_result.num_measurements - ); - // ── Check inversion ────────────────────────────────────────────── if !iter_result.uncertainty.inversion_succeeded { return Err(OutfitError::DifferentialCorrectionFailed( @@ -395,12 +394,6 @@ pub fn run_differential_correction( if num_selection_changes == 0 { break; } - - println!("last normalised RMS after outer pass {outer_pass}: {last_normalised_rms}"); - println!("number of selection changes in outlier rejection: {num_selection_changes}"); - println!("number of active measurements after outlier rejection: {last_num_measurements}"); - println!("stagnation count after outer pass {outer_pass}: {stagnation_count}"); - println!("\n ====== End of outer pass {outer_pass} ====== \n"); } // ── Final covariance rescaling ──────────────────────────────────────────── diff --git a/src/differential_orbit_correction/obs_dataset_api.rs b/src/differential_orbit_correction/obs_dataset_api.rs index 7b83977..d095edd 100644 --- a/src/differential_orbit_correction/obs_dataset_api.rs +++ b/src/differential_orbit_correction/obs_dataset_api.rs @@ -222,8 +222,6 @@ fn run_differential_correction_for_trajectory( } }; - println!("\n\nTrajectory {traj_id}: initial equinoctial elements: {initial_equinoctial:?}\n\n"); - // ── Build per-observation fit data from the error-model uncertainties ───── let obs_fit_data: Vec = observations .iter() @@ -240,8 +238,6 @@ fn run_differential_correction_for_trajectory( diff_cor_config, )?; - println!("\ndc_output for trajectory {traj_id}: {dc_output:?}\n"); - // ── Package the result ──────────────────────────────────────────────────── let orbital_elements = OrbitalElements::Equinoctial(dc_output.elements); Ok(FitOrbitResult::DifferentialCorrection(( diff --git a/src/differential_orbit_correction/single_iteration.rs b/src/differential_orbit_correction/single_iteration.rs index 70f2874..14e0d39 100644 --- a/src/differential_orbit_correction/single_iteration.rs +++ b/src/differential_orbit_correction/single_iteration.rs @@ -42,6 +42,7 @@ use crate::{ obs_fit_data::ObsFitData, }, observation_ephemeris::ObservationEphemeris, + propagator::PropagatorKind, EquinoctialElements, JPLEphem, OutfitError, }; @@ -135,6 +136,7 @@ pub struct SingleIterationResult { /// # Panics /// /// Panics if `observations.len() != obs_fit_data.len()`. +#[allow(clippy::too_many_arguments)] pub fn single_iteration( observations: &[Observation], obs_fit_data: &[ObsFitData], @@ -143,6 +145,7 @@ pub fn single_iteration( cache: &OutfitCache, jpl: &JPLEphem, apply_correction: bool, + propagator: &PropagatorKind, ) -> Result { assert_eq!( observations.len(), @@ -179,7 +182,15 @@ pub fn single_iteration( } // Propagate orbit and compute predicted position + element partials. - match obs.compute_obs_and_partials_2body(cache, jpl, elements) { + let partials_result = match propagator { + PropagatorKind::TwoBody => { + obs.compute_obs_and_partials_2body(cache, jpl, elements) + } + PropagatorKind::NBody(nbody_config) => { + obs.compute_obs_and_partials_nbody(cache, jpl, elements, nbody_config) + } + }; + match partials_result { Ok(partials) => { // Residual RA: angular difference in (−π, π] accounting for wrapping. // The observed RA is corrected for the catalogue bias before differencing. @@ -409,6 +420,7 @@ mod single_iteration_tests { &cache, &JPL_EPHEM_HORIZON, true, + &PropagatorKind::TwoBody, ) .unwrap(); @@ -450,6 +462,7 @@ mod single_iteration_tests { &cache, &JPL_EPHEM_HORIZON, false, // matonly + &PropagatorKind::TwoBody, ) .unwrap(); @@ -489,6 +502,7 @@ mod single_iteration_tests { &cache, &JPL_EPHEM_HORIZON, true, + &PropagatorKind::TwoBody, ) .unwrap(); @@ -524,6 +538,7 @@ mod single_iteration_tests { &cache, &JPL_EPHEM_HORIZON, true, + &PropagatorKind::TwoBody, ) .unwrap(); @@ -563,6 +578,7 @@ mod single_iteration_tests { &cache, &JPL_EPHEM_HORIZON, true, + &PropagatorKind::TwoBody, ) .unwrap(); @@ -600,6 +616,7 @@ mod single_iteration_tests { &cache, &JPL_EPHEM_HORIZON, true, + &PropagatorKind::TwoBody, ) .unwrap(); diff --git a/src/jpl_ephem/download_jpl_file.rs b/src/jpl_ephem/download_jpl_file.rs index 446a17e..036a5e5 100644 --- a/src/jpl_ephem/download_jpl_file.rs +++ b/src/jpl_ephem/download_jpl_file.rs @@ -208,7 +208,6 @@ impl EphemFileSource { /// * [`EphemFileSource::get_version_url`] — Compose the URL for a versioned file. pub async fn download_big_file(url: &str, path: &Utf8Path) -> Result<(), OutfitError> { let mut file = File::create(path).await?; - println!("Downloading {url}..."); let mut stream = reqwest::get(url).await?.bytes_stream(); @@ -219,7 +218,6 @@ pub async fn download_big_file(url: &str, path: &Utf8Path) -> Result<(), OutfitE file.flush().await?; - println!("Downloaded {url}"); Ok(()) } diff --git a/src/jpl_ephem/mod.rs b/src/jpl_ephem/mod.rs index 9b28297..5e9e4d0 100644 --- a/src/jpl_ephem/mod.rs +++ b/src/jpl_ephem/mod.rs @@ -72,7 +72,10 @@ use hifitime::Epoch; use horizon::{horizon_data::HorizonData, horizon_ids::HorizonID}; use naif::{ naif_data::NaifData, - naif_ids::{planet_bary::PlanetaryBary, solar_system_bary::SolarSystemBary, NaifIds}, + naif_ids::{ + planet_bary::PlanetaryBary, satellite_mass::SatelliteMassCenter, + solar_system_bary::SolarSystemBary, NaifIds, + }, }; use nalgebra::Vector3; @@ -187,6 +190,59 @@ impl JPLEphem { )), } } + + /// Return heliocentric position and velocity (AU, AU/day) of `body` at `epoch`. + /// + /// Works for both backends: + /// - **Horizon**: maps `NaifIds` to the corresponding `HorizonID` and queries relative to `Sun`. + /// - **NAIF**: queries `(body, SSB)` then subtracts `(Sun, SSB)` to obtain heliocentric state. + /// + /// # Errors + /// Returns [`OutfitError::EphemerisBodyNotSupported`] if the body cannot be mapped to + /// the active backend. + pub fn body_ephemeris( + &self, + body: NaifIds, + epoch: &Epoch, + ) -> Result<(Vector3, Vector3), OutfitError> { + match self { + JPLEphem::HorizonFile(horizon_data) => { + let horizon_id = naif_to_horizon_id(body)?; + let ephem_res = horizon_data + .ephemeris( + horizon_id, + HorizonID::Sun, + epoch.to_mjd_tt_days(), + true, + false, + ) + .to_au(); + let vel = ephem_res.velocity.unwrap_or_else(Vector3::zeros) * 86400.0; // AU/s → AU/day + Ok((ephem_res.position, vel)) + } + JPLEphem::NaifFile(naif_data) => { + let et = epoch.to_et_seconds(); + // Query body w.r.t. SSB + let body_res = naif_data + .ephemeris(body, NaifIds::SSB(SolarSystemBary::SSB), et) + .to_au(); + // Query Sun w.r.t. SSB + let sun_res = naif_data + .ephemeris( + NaifIds::SSB(SolarSystemBary::Sun), + NaifIds::SSB(SolarSystemBary::SSB), + et, + ) + .to_au(); + // Heliocentric = body_ssb - sun_ssb; convert AU/s → AU/day + let pos = body_res.position - sun_res.position; + let vel = (body_res.velocity.unwrap_or_else(Vector3::zeros) + - sun_res.velocity.unwrap_or_else(Vector3::zeros)) + / 86400.0; + Ok((pos, vel)) + } + } + } } impl TryFrom<&str> for JPLEphem { @@ -203,3 +259,29 @@ impl TryFrom for JPLEphem { JPLEphem::try_from(s.as_str()) } } + +/// Map a [`NaifIds`] to the corresponding [`HorizonID`] for the Horizon backend. +/// +/// Only bodies that are stored in JPL Horizon DE files are supported. +/// Returns [`OutfitError::EphemerisBodyNotSupported`] for anything else. +fn naif_to_horizon_id(body: NaifIds) -> Result { + match body { + NaifIds::SSB(SolarSystemBary::Sun) => Ok(HorizonID::Sun), + NaifIds::SSB(SolarSystemBary::SSB) => Err(OutfitError::EphemerisBodyNotSupported( + "Solar System Barycenter is not a physical body".to_string(), + )), + NaifIds::PB(PlanetaryBary::Mercury) => Ok(HorizonID::Mercury), + NaifIds::PB(PlanetaryBary::Venus) => Ok(HorizonID::Venus), + NaifIds::PB(PlanetaryBary::EarthMoon) => Ok(HorizonID::Earth), + NaifIds::PB(PlanetaryBary::Mars) => Ok(HorizonID::Mars), + NaifIds::PB(PlanetaryBary::Jupiter) => Ok(HorizonID::Jupiter), + NaifIds::PB(PlanetaryBary::Saturn) => Ok(HorizonID::Saturn), + NaifIds::PB(PlanetaryBary::Uranus) => Ok(HorizonID::Uranus), + NaifIds::PB(PlanetaryBary::Neptune) => Ok(HorizonID::Neptune), + NaifIds::PB(PlanetaryBary::Pluto) => Ok(HorizonID::Pluto), + NaifIds::SMC(SatelliteMassCenter::Moon) => Ok(HorizonID::Moon), + other => Err(OutfitError::EphemerisBodyNotSupported(format!( + "{other} is not available in the Horizon backend" + ))), + } +} diff --git a/src/lib.rs b/src/lib.rs index c5676ea..5d90e86 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -237,6 +237,9 @@ pub mod observation_ephemeris; /// Ground-observer geometry: body-fixed and heliocentric position routines. pub mod observer_extension; +/// Orbit propagation strategies (TwoBody, N-body DOP853). +pub mod propagator; + /// Core IOD pipeline trait over sorted observation slices. pub(crate) mod trajectory; diff --git a/src/observation_ephemeris.rs b/src/observation_ephemeris.rs index b2cbcce..b7576fb 100644 --- a/src/observation_ephemeris.rs +++ b/src/observation_ephemeris.rs @@ -28,6 +28,7 @@ use photom::{constants::DPI, observation_dataset::observation::Observation}; use crate::{ cache::OutfitCache, constants::{ROT_ECLMJ2000_TO_EQUMJ2000, ROT_EQUMJ2000_TO_ECLMJ2000}, + propagator::{nbody::propagate_nbody, NBodyConfig}, EquinoctialElements, JPLEphem, OutfitError, VLIGHT_AU, }; @@ -262,6 +263,19 @@ pub trait ObservationEphemeris { jpl: &JPLEphem, equinoctial_element: &EquinoctialElements, ) -> Result; + + /// Same as [`Self::compute_obs_and_partials_2body`] but uses a numerical N-body + /// propagator (DOP853) instead of the analytic Keplerian solution. + /// + /// The STM-based element Jacobian is computed via the variational equations + /// integrated alongside the state. + fn compute_obs_and_partials_nbody( + &self, + cache: &OutfitCache, + jpl: &JPLEphem, + equinoctial_element: &EquinoctialElements, + config: &NBodyConfig, + ) -> Result; } /// Compute topocentric (RA, DEC) and their partial derivatives w.r.t. the @@ -527,6 +541,68 @@ impl ObservationEphemeris for Observation { d_dec_d_elem, }) } + + fn compute_obs_and_partials_nbody( + &self, + cache: &OutfitCache, + jpl: &JPLEphem, + equinoctial_element: &EquinoctialElements, + config: &NBodyConfig, + ) -> Result { + // Hyperbolic/parabolic orbits are not yet supported + if equinoctial_element.eccentricity() >= 1.0 { + return Err(OutfitError::InvalidOrbit( + "Eccentricity >= 1 is not yet supported".to_string(), + )); + } + + // 1. N-body propagation to observation epoch + let t1_mjd_tt = self.mjd_tt(); + let nbody_result = propagate_nbody(equinoctial_element, t1_mjd_tt, jpl, config)?; + + // 2. Observation time in TT + let obs_mjd = Epoch::from_mjd_in_time_scale(t1_mjd_tt, hifitime::TimeScale::TT); + + // 3. Earth's barycentric position in ecliptic J2000 + let (earth_position, _) = jpl.earth_ephemeris(&obs_mjd, false); + let earth_pos_eclj2000 = ROT_EQUMJ2000_TO_ECLMJ2000 * earth_position; + let ast_pos_equ = ROT_ECLMJ2000_TO_EQUMJ2000 * nbody_result.position; + let ast_vel_equ = ROT_ECLMJ2000_TO_EQUMJ2000 * nbody_result.velocity; + + // 4. Observer heliocentric position in ecliptic J2000 + let geo_obs_pos = cache + .get_observer_geocentric_position(self.index()) + .map(|x| x.into_inner()); + let xobs = geo_obs_pos + earth_pos_eclj2000; + let obs_pos_equ = ROT_ECLMJ2000_TO_EQUMJ2000 * xobs; + + // 5. (α, δ) and ∂(α,δ)/∂pos (equatorial J2000) + let (ra, dec, d_ra_d_pos, d_dec_d_pos) = + topocentric_radec_and_partials(ast_pos_equ, ast_vel_equ, obs_pos_equ); + + // 6. Chain rule: same as two-body but using the STM-based Jacobian + let dpos_delem_eq = nbody_result.dpos_delem; // Matrix6x3, rows=elem, cols=ecl_cart + + let mut d_ra_d_elem = Vector6::zeros(); + let mut d_dec_d_elem = Vector6::zeros(); + for j in 0..6 { + let dpos_ecl_j = Vector3::new( + dpos_delem_eq[(j, 0)], + dpos_delem_eq[(j, 1)], + dpos_delem_eq[(j, 2)], + ); + let dpos_equ_j = ROT_ECLMJ2000_TO_EQUMJ2000 * dpos_ecl_j; + d_ra_d_elem[j] = d_ra_d_pos.dot(&dpos_equ_j); + d_dec_d_elem[j] = d_dec_d_pos.dot(&dpos_equ_j); + } + + Ok(ObsAndElementPartials { + ra, + dec, + d_ra_d_elem, + d_dec_d_elem, + }) + } } /// Apply stellar aberration correction to a relative position vector. diff --git a/src/outfit_errors.rs b/src/outfit_errors.rs index 580e0b4..b0debc4 100644 --- a/src/outfit_errors.rs +++ b/src/outfit_errors.rs @@ -278,6 +278,12 @@ pub enum OutfitError { /// divergence (e.g., normal-equation inversion failure). #[error("Differential correction failed: {0}")] DifferentialCorrectionFailed(String), + + #[error("Ephemeris body not supported by this backend: {0}")] + EphemerisBodyNotSupported(String), + + #[error("N-body propagation failed: {0}")] + NBodyPropagationFailed(String), } impl From<&ObsDatasetError> for OutfitError { @@ -364,6 +370,8 @@ impl PartialEq for OutfitError { (BizarreOrbit, BizarreOrbit) => true, (DifferentialCorrectionDiverged, DifferentialCorrectionDiverged) => true, (DifferentialCorrectionFailed(a), DifferentialCorrectionFailed(b)) => a == b, + (EphemerisBodyNotSupported(a), EphemerisBodyNotSupported(b)) => a == b, + (NBodyPropagationFailed(a), NBodyPropagationFailed(b)) => a == b, _ => false, } diff --git a/src/propagator/mod.rs b/src/propagator/mod.rs new file mode 100644 index 0000000..296aff6 --- /dev/null +++ b/src/propagator/mod.rs @@ -0,0 +1,62 @@ +//! Orbit propagation strategies for differential orbit correction. +//! +//! This module exposes two propagator kinds: +//! +//! - `PropagatorKind::TwoBody` – classic Keplerian propagation through the +//! analytic two-body solution. This is the default and requires no external +//! ephemeris data beyond the standard solar GM. +//! +//! - `PropagatorKind::NBody` – numerical N-body propagation via an 8th-order +//! Dormand-Prince (DOP853) integrator. The target object is integrated +//! together with its 6×6 state transition matrix (STM) under the influence of +//! the Sun and any additional perturbing bodies selected in `NBodyConfig`. +//! Planetary positions are looked up from the `JPLEphem` file supplied to the +//! differential corrector at runtime. + +pub mod nbody; +pub mod planet_gm; + +use crate::jpl_ephem::naif::naif_ids::NaifIds; + +/// Select which propagator is used during differential orbit correction. +#[derive(Debug, Clone, Default)] +pub enum PropagatorKind { + /// Keplerian (analytic) two-body propagation. Fast and self-contained. + #[default] + TwoBody, + + /// Numerical N-body propagation with user-specified perturbing bodies. + NBody(NBodyConfig), +} + +/// Configuration for the N-body propagator. +#[derive(Debug, Clone)] +pub struct NBodyConfig { + /// Bodies whose gravitational attraction perturbs the target orbit. + /// + /// Defaults to `[Sun]`. For each body the GM is taken from [`planet_gm`] + /// and its ephemeris is obtained from the `JPLEphem` file passed to the + /// integrator at runtime. + pub perturbing_bodies: Vec, + + /// Absolute tolerance for the DOP853 step-size control. + /// + /// Units: AU. Defaults to `1e-12`. + pub abs_tol: f64, + + /// Relative tolerance for the DOP853 step-size control. + /// + /// Defaults to `1e-12`. + pub rel_tol: f64, +} + +impl Default for NBodyConfig { + fn default() -> Self { + use crate::jpl_ephem::naif::naif_ids::solar_system_bary::SolarSystemBary; + Self { + perturbing_bodies: vec![NaifIds::SSB(SolarSystemBary::Sun)], + abs_tol: 1e-12, + rel_tol: 1e-12, + } + } +} diff --git a/src/propagator/nbody.rs b/src/propagator/nbody.rs new file mode 100644 index 0000000..1601672 --- /dev/null +++ b/src/propagator/nbody.rs @@ -0,0 +1,705 @@ +//! N-body propagator with state transition matrix (DOP853). +//! +//! # Overview +//! +//! This module integrates the heliocentric equations of motion for a small +//! solar-system body together with the 6×6 **state transition matrix** (STM) Φ — +//! the matrix of partial derivatives ∂(r,v)(t1)/∂(r,v)(t0) that maps small +//! perturbations of the initial state to perturbations of the final state, +//! using the explicit 8th-order Dormand–Prince (DOP853) integrator provided by +//! the [`differential-equations`](differential_equations) crate. +//! +//! ## State layout +//! The augmented state vector has 42 components stored as `[f64; 42]`: +//! +//! ```text +//! y[0..3] – heliocentric position r [AU] +//! y[3..6] – heliocentric velocity v [AU/day] +//! y[6..42] – STM Φ, stored column-major (nalgebra convention) as a flat 36-element slice +//! ``` +//! +//! ## Acceleration model +//! The Newtonian heliocentric acceleration for perturber `i` is +//! +//! ```text +//! a += −GMᵢ/|d|³ · d + GMᵢ/|rᵢ|³ · rᵢ (indirect term) +//! ``` +//! +//! where `d = r − rᵢ` is the asteroid–perturber vector, and the second term +//! removes the Sun's perturbation on the integration centre (heliocentric +//! formulation). When the Sun itself is a perturber the indirect term cancels +//! the direct term exactly, yielding the standard two-body acceleration. +//! +//! ## Variational equations +//! ```text +//! dΦ/dt = A(t) · Φ +//! ``` +//! with `A = [[0, I], [∂a/∂r, 0]]` and +//! +//! ```text +//! ∂a/∂r = Σᵢ GMᵢ · (3 d dᵀ/|d|⁵ − I/|d|³) +//! ``` +//! +//! ## Element Jacobian +//! The result of this module is `dpos_delem_ecl` (Matrix6x3, rows = elements, +//! cols = ecliptic Cartesian components) that is directly substitutable for the +//! matrix returned by `solve_two_body_problem`. It is computed as +//! +//! ```text +//! J(t1) = [Φ_pp | Φ_pv] · J0 (top 3 rows of Φ(t1) · J0_full) +//! ``` +//! +//! where `J0_full` is the 6×6 matrix formed by stacking `dpos_delem` and +//! `dvel_delem` (each 6×3) from a zero-propagation call to +//! `solve_two_body_problem`. + +use differential_equations::ode::ODE; +use differential_equations::prelude::*; +use nalgebra::{Matrix3, Matrix6, Matrix6x3, Vector3}; + +use crate::{ + jpl_ephem::JPLEphem, orbit_type::equinoctial_element::EquinoctialElements, + outfit_errors::OutfitError, +}; + +use super::{planet_gm::gm_au3_day2, NBodyConfig}; + +// --------------------------------------------------------------------------- +// ODE right-hand side +// --------------------------------------------------------------------------- + +// The augmented state is [f64; 42]: +// y[0..3] = position (AU) +// y[3..6] = velocity (AU/day) +// y[6..42] = STM Φ stored col-major + +/// Snapshot of a perturbing body at a fixed epoch. +/// +/// The snapshot is taken at t0 and held constant over the integration arc. +/// This is accurate for short arcs (≲ 30 days) where planetary motion is slow. +struct PerturberSnapshot { + /// Heliocentric position of the perturber at t0, in the ecliptic J2000 frame. + /// + /// Units: AU. + heliocentric_position: Vector3, + + /// Gravitational parameter GM of the perturber. + /// + /// Units: AU³/day². + gravitational_parameter: f64, +} + +/// ODE right-hand side for the augmented state (position, velocity, STM). +/// +/// Implements [`ODE`] so that it can be driven by the DOP853 +/// integrator. The dynamics are frozen at t0: perturber positions are sampled +/// once and held constant throughout the integration arc. +struct NBodyOde { + /// Perturber snapshots evaluated at t0. + /// + /// Each entry holds the heliocentric position and GM of one perturbing body. + /// These values are used on every call to [`ODE::diff`] to compute the total + /// heliocentric acceleration and the gravity-gradient matrix required by the + /// variational equations. + perturbers: Vec, +} + +// --------------------------------------------------------------------------- +// Acceleration sub-functions +// --------------------------------------------------------------------------- + +/// Computes the direct gravitational acceleration exerted by a single perturber +/// on the small body: +/// +/// ```text +/// a_direct = −GM / |asteroid_to_perturber|³ · asteroid_to_perturber +/// ``` +/// +/// # Arguments +/// +/// * `asteroid_to_perturber` – Vector from the heliocentric position of the +/// small body to the heliocentric position of the perturber, i.e. +/// `r_perturber − r_asteroid`. Units: AU. +/// * `gravitational_parameter` – Gravitational parameter GM of the perturber. +/// Units: AU³/day². +/// +/// # Returns +/// +/// The direct acceleration vector (AU/day²) pointing from the small body +/// toward the perturber, scaled by GM / |d|³. +fn direct_acceleration( + asteroid_to_perturber: Vector3, + gravitational_parameter: f64, +) -> Vector3 { + let distance = asteroid_to_perturber.norm(); + let distance_cubed = distance.powi(3); + -gravitational_parameter / distance_cubed * asteroid_to_perturber +} + +/// Computes the indirect (heliocentric frame correction) acceleration from a +/// single perturber: +/// +/// ```text +/// a_indirect = +GM / |perturber_heliocentric_pos|³ · perturber_heliocentric_pos +/// ``` +/// +/// Returns zero if the perturber is at (or very near) the origin (i.e. it is +/// the Sun itself, which cancels with its direct term). +/// +/// # Arguments +/// +/// * `perturber_heliocentric_position` – Heliocentric position of the perturber +/// in the ecliptic J2000 frame. Units: AU. +/// * `gravitational_parameter` – Gravitational parameter GM of the perturber. +/// Units: AU³/day². +/// +/// # Returns +/// +/// The indirect acceleration correction vector (AU/day²) that removes the +/// apparent acceleration of the heliocentric origin due to the perturber. +/// Returns [`Vector3::zeros`] when the perturber distance is ≤ 1 × 10⁻¹⁰ AU +/// (i.e. the perturber coincides with the Sun). +fn indirect_acceleration( + perturber_heliocentric_position: Vector3, + gravitational_parameter: f64, +) -> Vector3 { + let perturber_distance = perturber_heliocentric_position.norm(); + if perturber_distance > 1e-10 { + let perturber_distance_cubed = perturber_distance.powi(3); + gravitational_parameter / perturber_distance_cubed * perturber_heliocentric_position + } else { + Vector3::zeros() + } +} + +/// Computes the gravity-gradient matrix contribution (∂a/∂r) from a single +/// perturber: +/// +/// ```text +/// ∂a/∂r += −GM · (I/|d|³ − 3 d dᵀ/|d|⁵) +/// ``` +/// +/// # Arguments +/// +/// * `asteroid_to_perturber` – Vector from the heliocentric position of the +/// small body to that of the perturber (`r_perturber − r_asteroid`). +/// Units: AU. +/// * `gravitational_parameter` – Gravitational parameter GM of the perturber. +/// Units: AU³/day². +/// +/// # Returns +/// +/// A 3×3 matrix (units: day⁻²) representing this perturber's contribution to +/// ∂a/∂r, the partial derivative of the acceleration with respect to the +/// small-body position. Summing this matrix over all perturbers yields the +/// lower-left block of the linearised dynamics matrix A used in the variational +/// equations `dΦ/dt = A · Φ`. +fn gravity_gradient_contribution( + asteroid_to_perturber: Vector3, + gravitational_parameter: f64, +) -> Matrix3 { + let distance = asteroid_to_perturber.norm(); + let distance_cubed = distance.powi(3); + let distance_fifth = distance.powi(5); + let outer_product = asteroid_to_perturber * asteroid_to_perturber.transpose(); + -gravitational_parameter + * (Matrix3::::identity() * (1.0 / distance_cubed) + - outer_product * (3.0 / distance_fifth)) +} + +/// Accumulates total heliocentric acceleration and gravity-gradient matrix +/// over all perturbers. +/// +/// Iterates over every [`PerturberSnapshot`] and sums the direct acceleration, +/// indirect acceleration correction, and gravity-gradient contribution from +/// each body. +/// +/// # Arguments +/// +/// * `asteroid_heliocentric_position` – Heliocentric position of the small body +/// in the ecliptic J2000 frame. Units: AU. +/// * `perturbers` – Slice of perturber snapshots evaluated at t0. Each entry +/// provides the perturber's heliocentric position (AU) and GM (AU³/day²). +/// +/// # Returns +/// +/// A tuple `(total_acceleration, da_dr)` where: +/// - `total_acceleration` – Combined heliocentric acceleration vector (AU/day²) +/// from all perturbers, including indirect terms. +/// - `da_dr` – 3×3 gravity-gradient matrix (day⁻²), i.e. the sum of each +/// perturber's ∂a/∂r contribution, used to build the linearised dynamics +/// matrix for the STM variational equations. +fn accumulate_perturber_effects( + asteroid_heliocentric_position: Vector3, + perturbers: &[PerturberSnapshot], +) -> (Vector3, Matrix3) { + perturbers.iter().fold( + (Vector3::zeros(), Matrix3::zeros()), + |(acc_total, da_dr_total), perturber| { + let asteroid_to_perturber = + asteroid_heliocentric_position - perturber.heliocentric_position; + + let acc_direct = + direct_acceleration(asteroid_to_perturber, perturber.gravitational_parameter); + let acc_indirect = indirect_acceleration( + perturber.heliocentric_position, + perturber.gravitational_parameter, + ); + let da_dr_contribution = gravity_gradient_contribution( + asteroid_to_perturber, + perturber.gravitational_parameter, + ); + + ( + acc_total + acc_direct + acc_indirect, + da_dr_total + da_dr_contribution, + ) + }, + ) +} + +/// Builds the 6×6 linearised dynamics matrix A from the gravity-gradient block: +/// +/// ```text +/// A = | 0 I | +/// | ∂a/∂r 0 | +/// ``` +/// +/// # Arguments +/// +/// * `gravity_gradient` – 3×3 gravity-gradient matrix ∂a/∂r (day⁻²), as +/// returned by [`accumulate_perturber_effects`]. It occupies the lower-left +/// 3×3 block of A. +/// +/// # Returns +/// +/// The 6×6 linearised dynamics matrix A (units: mixed — upper-right block is +/// dimensionless identity, lower-left block has units day⁻²). This matrix +/// satisfies the variational equation `dΦ/dt = A · Φ`. +fn build_dynamics_matrix(gravity_gradient: Matrix3) -> Matrix6 { + let mut dynamics_matrix = Matrix6::::zeros(); + // Upper-right 3×3 block: identity (velocity → position coupling) + for i in 0..3 { + dynamics_matrix[(i, i + 3)] = 1.0; + } + // Lower-left 3×3 block: ∂a/∂r (acceleration → velocity coupling) + for row in 0..3 { + for col in 0..3 { + dynamics_matrix[(row + 3, col)] = gravity_gradient[(row, col)]; + } + } + dynamics_matrix +} + +/// Writes position and velocity derivatives into the first 6 components of +/// `state_derivative` from the current velocity and computed acceleration. +/// +/// # Arguments +/// +/// * `current_velocity` – Full 42-element augmented state vector. Indices 3–5 +/// are used as the current velocity components (AU/day), which become the +/// time derivative of position. +/// * `acceleration` – Total heliocentric acceleration vector (AU/day²) +/// previously computed by [`accumulate_perturber_effects`]. +/// * `state_derivative` – Mutable reference to the 42-element output derivative +/// array. On return, indices 0–2 contain `ṙ = v` (AU/day) and indices 3–5 +/// contain `v̇ = a` (AU/day²). +fn write_position_velocity_derivatives( + current_velocity: &[f64; 42], + acceleration: Vector3, + state_derivative: &mut [f64; 42], +) { + state_derivative[0] = current_velocity[3]; + state_derivative[1] = current_velocity[4]; + state_derivative[2] = current_velocity[5]; + state_derivative[3] = acceleration[0]; + state_derivative[4] = acceleration[1]; + state_derivative[5] = acceleration[2]; +} + +/// Computes dΦ/dt = A · Φ and writes the result into `state_derivative[6..42]`. +/// +/// # Arguments +/// +/// * `dynamics_matrix` – 6×6 linearised dynamics matrix A built by +/// [`build_dynamics_matrix`]. Units: mixed (see that function's documentation). +/// * `augmented_state` – Full 42-element augmented state vector. Indices 6–41 +/// hold the current STM Φ stored in column-major order. +/// * `state_derivative` – Mutable reference to the 42-element output derivative +/// array. On return, indices 6–41 contain the flattened (column-major) entries +/// of dΦ/dt = A · Φ. +fn write_stm_derivative( + dynamics_matrix: Matrix6, + augmented_state: &[f64; 42], + state_derivative: &mut [f64; 42], +) { + let phi = Matrix6::::from_column_slice(&augmented_state[6..42]); + let dphi_dt = dynamics_matrix * phi; + state_derivative[6..42].copy_from_slice(dphi_dt.as_slice()); +} + +impl ODE for NBodyOde { + fn diff(&self, _time: f64, augmented_state: &[f64; 42], state_derivative: &mut [f64; 42]) { + let asteroid_heliocentric_position = + Vector3::new(augmented_state[0], augmented_state[1], augmented_state[2]); + + let (total_acceleration, gravity_gradient) = + accumulate_perturber_effects(asteroid_heliocentric_position, &self.perturbers); + + write_position_velocity_derivatives(augmented_state, total_acceleration, state_derivative); + + let dynamics_matrix = build_dynamics_matrix(gravity_gradient); + write_stm_derivative(dynamics_matrix, augmented_state, state_derivative); + } +} + +// --------------------------------------------------------------------------- +// Public interface +// --------------------------------------------------------------------------- + +/// Result of a single N-body propagation step. +pub struct NBodyResult { + /// Heliocentric position of the small body at t1, in the ecliptic J2000 frame. + /// + /// Units: AU. + pub position: Vector3, + + /// Heliocentric velocity of the small body at t1, in the ecliptic J2000 frame. + /// + /// Units: AU/day. + pub velocity: Vector3, + + /// Partial derivatives of the propagated position with respect to the six + /// equinoctial orbital elements, evaluated at t1. + /// + /// Shape: 6×3 (rows = equinoctial elements `[a, h, k, p, q, λ]`, + /// cols = ecliptic Cartesian components `[x, y, z]`). + /// Units: AU / (element unit). + pub dpos_delem: Matrix6x3, + + /// Partial derivatives of the propagated velocity with respect to the six + /// equinoctial orbital elements, evaluated at t1. + /// + /// Shape: 6×3 (rows = equinoctial elements `[a, h, k, p, q, λ]`, + /// cols = ecliptic Cartesian velocity components `[ẋ, ẏ, ż]`). + /// Units: (AU/day) / (element unit). + pub dvel_delem: Matrix6x3, +} + +// --------------------------------------------------------------------------- +// propagate_nbody sub-functions +// --------------------------------------------------------------------------- + +/// Builds the augmented initial state vector `[pos, vel, vec(Φ=I₆)]`. +/// +/// Packs position and velocity into indices 0–5 and initialises the STM block +/// (indices 6–41) to the identity matrix I₆ stored in column-major order, so +/// that the integration starts with Φ(t0) = I. +/// +/// # Arguments +/// +/// * `initial_position` – Heliocentric position of the small body at t0. +/// Units: AU. +/// * `initial_velocity` – Heliocentric velocity of the small body at t0. +/// Units: AU/day. +/// +/// # Returns +/// +/// A 42-element array `[f64; 42]` with layout: +/// - indices 0–2: position components (AU), +/// - indices 3–5: velocity components (AU/day), +/// - indices 6–41: column-major entries of Φ₀ = I₆ (dimensionless). +fn build_augmented_initial_state( + initial_position: Vector3, + initial_velocity: Vector3, +) -> [f64; 42] { + let mut augmented_state = [0.0_f64; 42]; + augmented_state[0] = initial_position[0]; + augmented_state[1] = initial_position[1]; + augmented_state[2] = initial_position[2]; + augmented_state[3] = initial_velocity[0]; + augmented_state[4] = initial_velocity[1]; + augmented_state[5] = initial_velocity[2]; + // Φ₀ = I₆ stored col-major + augmented_state[6..42].copy_from_slice(Matrix6::::identity().as_slice()); + augmented_state +} + +/// Queries the ephemeris and builds a perturber snapshot vector at the given +/// epoch. +/// +/// For each body listed in [`NBodyConfig::perturbing_bodies`], this function +/// looks up the gravitational parameter from the static table in +/// [`planet_gm`](super::planet_gm) and queries the heliocentric position from +/// the supplied JPL ephemeris file. +/// +/// # Arguments +/// +/// * `config` – N-body configuration specifying which perturbing bodies to +/// include and the integrator tolerances. +/// * `jpl` – Opened JPL ephemeris file used to query the heliocentric position +/// of each perturbing body. +/// * `epoch` – Reference epoch at which perturber positions are sampled. +/// Passed directly to [`JPLEphem::body_ephemeris`]. +/// +/// # Returns +/// +/// A [`Vec`] with one entry per body listed in +/// `config.perturbing_bodies`, ordered identically. Each entry contains the +/// body's heliocentric position (AU) and GM (AU³/day²) at the given epoch. +/// +/// # Errors +/// +/// - Returns [`OutfitError::EphemerisBodyNotSupported`] if a perturbing body +/// has no GM entry in the static table or cannot be resolved by the JPL +/// ephemeris file. +fn build_perturber_snapshots( + config: &NBodyConfig, + jpl: &JPLEphem, + epoch: &hifitime::Epoch, +) -> Result, OutfitError> { + config + .perturbing_bodies + .iter() + .map(|&body| { + let gravitational_parameter = gm_au3_day2(body).ok_or_else(|| { + OutfitError::EphemerisBodyNotSupported(format!( + "No GM available for perturber {body:?}" + )) + })?; + let (heliocentric_position, _velocity) = jpl.body_ephemeris(body, epoch)?; + Ok(PerturberSnapshot { + heliocentric_position, + gravitational_parameter, + }) + }) + .collect() +} + +/// Runs the DOP853 integrator from t=0 to t=`time_span_days` and returns the +/// final augmented state vector. +/// +/// Constructs a DOP853 initial-value problem from the provided ODE, integrates +/// from time 0 to `time_span_days` using the absolute and relative tolerances +/// specified in `config`, and returns the last computed augmented state. +/// +/// # Arguments +/// +/// * `ode` – Reference to the [`NBodyOde`] instance holding the frozen perturber +/// snapshots. Implements the ODE right-hand side. +/// * `augmented_initial_state` – 42-element initial augmented state vector at +/// t=0, as produced by [`build_augmented_initial_state`]. +/// * `time_span_days` – Integration duration. Positive for forward propagation, +/// negative for backward propagation. Units: days. +/// * `config` – N-body configuration supplying `abs_tol` (AU) and `rel_tol` +/// (dimensionless) for the adaptive step-size control of DOP853. +/// +/// # Returns +/// +/// The 42-element augmented state at t=`time_span_days`: +/// - indices 0–2: propagated position (AU), +/// - indices 3–5: propagated velocity (AU/day), +/// - indices 6–41: column-major entries of Φ(t1) (dimensionless). +/// +/// # Errors +/// +/// - Returns [`OutfitError::NBodyPropagationFailed`] if the DOP853 solver +/// returns an error or if the solution contains no steps. +fn integrate_augmented_state( + ode: &NBodyOde, + augmented_initial_state: [f64; 42], + time_span_days: f64, + config: &NBodyConfig, +) -> Result<[f64; 42], OutfitError> { + let solution = IVP::ode(ode, 0.0_f64, time_span_days, augmented_initial_state) + .method( + ExplicitRungeKutta::dop853() + .atol(config.abs_tol) + .rtol(config.rel_tol), + ) + .solve() + .map_err(|e| OutfitError::NBodyPropagationFailed(format!("{e:?}")))?; + + solution.y.last().copied().ok_or_else(|| { + OutfitError::NBodyPropagationFailed("Integrator returned no steps".to_string()) + }) +} + +/// Converts the t0 element Jacobians `(dpos_delem0, dvel_delem0)` (each 6×3) +/// into a single 6×6 matrix whose rows are state components and whose columns +/// are element indices: +/// +/// ```text +/// J0 = | (dpos_delem0)ᵀ | +/// | (dvel_delem0)ᵀ | +/// ``` +/// +/// # Arguments +/// +/// * `dpos_delem_at_t0` – 6×3 Jacobian of heliocentric position with respect to +/// the six equinoctial elements at t0 (rows = elements, cols = Cartesian +/// components). Units: AU / (element unit). +/// * `dvel_delem_at_t0` – 6×3 Jacobian of heliocentric velocity with respect to +/// the six equinoctial elements at t0 (rows = elements, cols = Cartesian +/// velocity components). Units: (AU/day) / (element unit). +/// +/// # Returns +/// +/// A 6×6 matrix J0 where: +/// - rows 0–2 correspond to the three position Cartesian components, +/// - rows 3–5 correspond to the three velocity Cartesian components, +/// - column `j` corresponds to equinoctial element index `j`. +/// +/// This layout is compatible with the STM Φ so that the product `Φ(t1) · J0` +/// propagates the element Jacobian to t1. +fn build_initial_state_jacobian( + dpos_delem_at_t0: &Matrix6x3, + dvel_delem_at_t0: &Matrix6x3, +) -> Matrix6 { + let mut initial_state_jacobian = Matrix6::::zeros(); + for element_index in 0..6 { + for cartesian_component in 0..3 { + initial_state_jacobian[(cartesian_component, element_index)] = + dpos_delem_at_t0[(element_index, cartesian_component)]; + initial_state_jacobian[(cartesian_component + 3, element_index)] = + dvel_delem_at_t0[(element_index, cartesian_component)]; + } + } + initial_state_jacobian +} + +/// Splits the propagated 6×6 Jacobian `Φ(t1) · J0` back into the +/// `(dpos_delem, dvel_delem)` pair at t1 (each 6×3). +/// +/// Performs the inverse of the layout established by [`build_initial_state_jacobian`]: +/// extracts the top 3 rows into `dpos_delem_at_t1` and the bottom 3 rows into +/// `dvel_delem_at_t1`, transposing the index ordering back to the 6×3 convention +/// (rows = elements, cols = Cartesian components). +/// +/// # Arguments +/// +/// * `propagated_state_jacobian` – 6×6 matrix `Φ(t1) · J0` resulting from +/// multiplying the STM at t1 by the initial-state Jacobian J0. +/// - rows 0–2: propagated position partials, +/// - rows 3–5: propagated velocity partials, +/// - column `j`: partial with respect to equinoctial element `j`. +/// +/// # Returns +/// +/// A tuple `(dpos_delem_at_t1, dvel_delem_at_t1)` where each matrix has shape +/// 6×3 (rows = equinoctial elements, cols = ecliptic Cartesian components): +/// - `dpos_delem_at_t1`: ∂pos(t1)/∂elem. Units: AU / (element unit). +/// - `dvel_delem_at_t1`: ∂vel(t1)/∂elem. Units: (AU/day) / (element unit). +fn split_propagated_jacobian( + propagated_state_jacobian: Matrix6, +) -> (Matrix6x3, Matrix6x3) { + let mut dpos_delem_at_t1 = Matrix6x3::::zeros(); + let mut dvel_delem_at_t1 = Matrix6x3::::zeros(); + for element_index in 0..6 { + for cartesian_component in 0..3 { + dpos_delem_at_t1[(element_index, cartesian_component)] = + propagated_state_jacobian[(cartesian_component, element_index)]; + dvel_delem_at_t1[(element_index, cartesian_component)] = + propagated_state_jacobian[(cartesian_component + 3, element_index)]; + } + } + (dpos_delem_at_t1, dvel_delem_at_t1) +} + +/// Propagates `elements` from their reference epoch to `t1_mjd_tt` using +/// numerical N-body integration. +/// +/// Converts the equinoctial elements to a Cartesian initial state at t0, then +/// integrates the augmented state (position, velocity, and 6×6 STM) forward (or +/// backward) from t0 to t1 using the DOP853 integrator under the gravitational +/// influence of all bodies listed in `config.perturbing_bodies`. On completion, +/// the propagated STM is combined with the initial element Jacobians to yield +/// `dpos_delem` and `dvel_delem` at t1. +/// +/// If `|t1 − t0| < 1 × 10⁻¹⁴` days the integration is skipped and the t0 +/// Cartesian state and Jacobians are returned directly. +/// +/// # Arguments +/// +/// * `elements` – Equinoctial orbital elements of the small body, with +/// `reference_epoch` (MJD-TT) acting as t0. Units: AU, radians. +/// * `t1_mjd_tt` – Target epoch expressed as Modified Julian Date in the +/// Terrestrial Time (TT) scale. Units: days (MJD-TT). +/// * `jpl` – Opened JPL ephemeris file used to query the heliocentric positions +/// of the perturbing bodies at t0. +/// * `config` – N-body configuration specifying the perturbing bodies and the +/// DOP853 absolute/relative tolerances. +/// +/// # Returns +/// +/// An [`NBodyResult`] containing: +/// - `position`: heliocentric position at t1 (AU, ecliptic J2000), +/// - `velocity`: heliocentric velocity at t1 (AU/day, ecliptic J2000), +/// - `dpos_delem`: 6×3 Jacobian ∂pos(t1)/∂elem (rows = elements, cols = Cartesian), +/// - `dvel_delem`: 6×3 Jacobian ∂vel(t1)/∂elem (rows = elements, cols = Cartesian). +/// +/// # Errors +/// +/// - Returns [`OutfitError::NBodyPropagationFailed`] if the DOP853 integrator +/// fails or produces no output steps. +/// - Returns [`OutfitError::EphemerisBodyNotSupported`] if a perturbing body +/// listed in `config` cannot be found in the JPL ephemeris or has no GM entry +/// in the static table. +pub fn propagate_nbody( + elements: &EquinoctialElements, + t1_mjd_tt: f64, + jpl: &JPLEphem, + config: &NBodyConfig, +) -> Result { + let t0_mjd_tt = elements.reference_epoch; + let time_span_days = t1_mjd_tt - t0_mjd_tt; + + // Initial cartesian state and element Jacobian J0 = ∂(x0,v0)/∂elem + let (initial_position, initial_velocity, initial_jacobians) = + elements.solve_two_body_problem(0.0, 0.0, true)?; + let (dpos_delem_at_t0, dvel_delem_at_t0) = + initial_jacobians.expect("solve_two_body_problem(0,0,true) must return jacobians"); + + // If dt ≈ 0 skip integration + if time_span_days.abs() < 1e-14 { + return Ok(NBodyResult { + position: initial_position, + velocity: initial_velocity, + dpos_delem: dpos_delem_at_t0, + dvel_delem: dvel_delem_at_t0, + }); + } + + let augmented_initial_state = build_augmented_initial_state(initial_position, initial_velocity); + + let epoch_t0 = hifitime::Epoch::from_mjd_in_time_scale(t0_mjd_tt, hifitime::TimeScale::TT); + let perturbers = build_perturber_snapshots(config, jpl, &epoch_t0)?; + + let ode = NBodyOde { perturbers }; + + let augmented_final_state = + integrate_augmented_state(&ode, augmented_initial_state, time_span_days, config)?; + + let final_position = Vector3::new( + augmented_final_state[0], + augmented_final_state[1], + augmented_final_state[2], + ); + let final_velocity = Vector3::new( + augmented_final_state[3], + augmented_final_state[4], + augmented_final_state[5], + ); + let stm_at_t1 = Matrix6::::from_column_slice(&augmented_final_state[6..42]); + + let initial_state_jacobian = build_initial_state_jacobian(&dpos_delem_at_t0, &dvel_delem_at_t0); + let propagated_state_jacobian = stm_at_t1 * initial_state_jacobian; + let (dpos_delem_at_t1, dvel_delem_at_t1) = split_propagated_jacobian(propagated_state_jacobian); + + Ok(NBodyResult { + position: final_position, + velocity: final_velocity, + dpos_delem: dpos_delem_at_t1, + dvel_delem: dvel_delem_at_t1, + }) +} diff --git a/src/propagator/planet_gm.rs b/src/propagator/planet_gm.rs new file mode 100644 index 0000000..4407279 --- /dev/null +++ b/src/propagator/planet_gm.rs @@ -0,0 +1,95 @@ +//! Gravitational parameters (GM) for solar-system bodies. +//! +//! All values are in **AU³/day²**, consistent with the Gaussian gravitational +//! constant `k` used throughout Outfit (`k² = GAUSS_GRAV_SQUARED`). +//! +//! Sources: DE440 TDB-compatible mass parameters converted to AU³/day² via +//! `GM_km3_s2 * (86400² / AU³_in_km³)` where `AU = 1.495978707e8 km`. +//! +//! # References +//! - Park, R.S. et al. (2021), *The JPL Planetary and Lunar Ephemerides DE440 and DE441*, +//! AJ 161, 105. + +use crate::jpl_ephem::naif::naif_ids::{ + planet_bary::PlanetaryBary, satellite_mass::SatelliteMassCenter, + solar_system_bary::SolarSystemBary, NaifIds, +}; + +/// AU in kilometres (IAU 2012). +const AU_KM: f64 = 1.495_978_707e8; + +/// Conversion factor: km³/s² → AU³/day² +/// = (86400 s/day)² / (AU_KM km/AU)³ +const KM3_S2_TO_AU3_DAY2: f64 = (86400.0 * 86400.0) / (AU_KM * AU_KM * AU_KM); + +// --------------------------------------------------------------------------- +// Individual GM values in km³/s² (DE440) +// --------------------------------------------------------------------------- + +const GM_SUN_KM3_S2: f64 = 1.327_124_400_41e11; +const GM_MERCURY_KM3_S2: f64 = 2.203_178_e4; +const GM_VENUS_KM3_S2: f64 = 3.248_585_7e5; +/// Earth + Moon system GM (used for EarthMoon barycenter). +const GM_EARTH_MOON_KM3_S2: f64 = 4.035_032_35e5; +const GM_MARS_KM3_S2: f64 = 4.282_837_36e4; +const GM_JUPITER_KM3_S2: f64 = 1.267_127_648e8; +const GM_SATURN_KM3_S2: f64 = 3.794_062_52e7; +const GM_URANUS_KM3_S2: f64 = 5.794_556_4e6; +const GM_NEPTUNE_KM3_S2: f64 = 6.836_527_1e6; +const GM_PLUTO_KM3_S2: f64 = 9.755_e2; +const GM_MOON_KM3_S2: f64 = 4.902_800_066e3; + +// --------------------------------------------------------------------------- +// Public AU³/day² constants +// --------------------------------------------------------------------------- + +pub const GM_SUN: f64 = GM_SUN_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_MERCURY: f64 = GM_MERCURY_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_VENUS: f64 = GM_VENUS_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_EARTH_MOON: f64 = GM_EARTH_MOON_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_MARS: f64 = GM_MARS_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_JUPITER: f64 = GM_JUPITER_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_SATURN: f64 = GM_SATURN_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_URANUS: f64 = GM_URANUS_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_NEPTUNE: f64 = GM_NEPTUNE_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_PLUTO: f64 = GM_PLUTO_KM3_S2 * KM3_S2_TO_AU3_DAY2; +pub const GM_MOON: f64 = GM_MOON_KM3_S2 * KM3_S2_TO_AU3_DAY2; + +/// Return the GM (AU³/day²) for a [`NaifIds`], or `None` if not tabulated. +pub fn gm_au3_day2(body: NaifIds) -> Option { + match body { + NaifIds::SSB(SolarSystemBary::Sun) => Some(GM_SUN), + NaifIds::SSB(SolarSystemBary::SSB) => None, + NaifIds::PB(PlanetaryBary::Mercury) => Some(GM_MERCURY), + NaifIds::PB(PlanetaryBary::Venus) => Some(GM_VENUS), + NaifIds::PB(PlanetaryBary::EarthMoon) => Some(GM_EARTH_MOON), + NaifIds::PB(PlanetaryBary::Mars) => Some(GM_MARS), + NaifIds::PB(PlanetaryBary::Jupiter) => Some(GM_JUPITER), + NaifIds::PB(PlanetaryBary::Saturn) => Some(GM_SATURN), + NaifIds::PB(PlanetaryBary::Uranus) => Some(GM_URANUS), + NaifIds::PB(PlanetaryBary::Neptune) => Some(GM_NEPTUNE), + NaifIds::PB(PlanetaryBary::Pluto) => Some(GM_PLUTO), + NaifIds::SMC(SatelliteMassCenter::Moon) => Some(GM_MOON), + _ => None, + } +} + +#[cfg(test)] +mod planet_gm_tests { + use super::*; + + #[test] + fn sun_gm_close_to_gauss_squared() { + // k² = 2.9591220828e-4 AU³/day² (Gaussian) + // DE440 GM_Sun should be within 1e-8 relative of the Gaussian value + let gauss_k2 = crate::constants::GAUSS_GRAV_SQUARED; + let rel = (GM_SUN - gauss_k2).abs() / gauss_k2; + assert!(rel < 1e-4, "GM_SUN rel diff from k²: {rel:.2e}"); + } + + #[test] + fn jupiter_dominates_perturbations() { + // Jupiter GM should be ~1000× larger than Mars GM + const { assert!(GM_JUPITER > GM_MARS * 100.0) }; + } +} diff --git a/tests/test_diff_cor.rs b/tests/test_diff_cor.rs index 4d9f111..be26c06 100644 --- a/tests/test_diff_cor.rs +++ b/tests/test_diff_cor.rs @@ -3,8 +3,12 @@ mod common; use crate::common::approx_equal; use approx::assert_relative_eq; use hifitime::ut1::Ut1Provider; +use outfit::jpl_ephem::naif::naif_ids::{ + planet_bary::PlanetaryBary, solar_system_bary::SolarSystemBary, NaifIds, +}; use outfit::{ orbit_type::{equinoctial_element::EquinoctialElements, OrbitalElements}, + propagator::{NBodyConfig, PropagatorKind}, DifferentialCorrectionConfig, FitLSQ, IODParams, JPLEphem, }; use photom::TrajId; @@ -45,19 +49,6 @@ fn build_test_fixtures() -> ( // NOTE: rms_divergence_ratio is raised above the default (1.5) to allow // the 2-body differential corrector to handle objects whose osculating // 2-body elements differ noticeably from the N-body solution (e.g. 8467). - // - // Root cause: Outfit uses pure 2-body (Keplerian) propagation during - // differential correction. For well-observed main-belt asteroids the - // best-fit 2-body elements differ from the best-fit N-body elements. When - // the IOD starting point happens to be close to the N-body solution, the - // first Newton step moves the orbit toward the 2-body minimum — which is - // further from the data in N-body residuals — causing the RMS to jump by - // more than the default 1.5× threshold before the iteration has a chance - // to settle. Setting the ratio to 10 lets the iteration continue through - // that transient increase. - // - // Long-term fix: implement N-body propagation + variational equations in - // the differential corrector (tracked separately). let diff_cor_config = DifferentialCorrectionConfig { rms_divergence_ratio: 10.0, ..DifferentialCorrectionConfig::default() @@ -176,3 +167,305 @@ fn test_diff_cor() { assert_relative_eq!(orbit.orbit_quality(), 3.450e-1, max_relative = 1e-3); } } + +/// N-body differential orbit correction test. +/// +/// Runs the same dataset with Sun + Jupiter as perturbers and verifies: +/// +/// 1. All three objects still converge. +/// 2. The fitted semi-major axes are physically consistent with the two-body +/// oracle (no sign flip, reasonable magnitude). +/// 3. For the shortest arc (8467, ~40 days) the N-body elements agree with +/// the two-body oracle within 5e-2 AU / 5e-2 (eccentricity components). +/// Jupiter's perturbation over 40 days at 3.2 AU is measurable but small. +/// 4. The orbit quality (normalised RMS) remains below a generous bound of 5.0 +/// for all objects, confirming the fit converged to a good residual level. +/// +/// The test intentionally does **not** demand tight element agreement for the +/// longer arcs (2015AB at ~5 years, 33803 at ~5 months), where the best-fit +/// N-body and two-body elements differ by physical amounts (Jovian +/// perturbations accumulate over the arc). What it does verify is that the +/// propagator produces a self-consistent, converged orbit. +#[test] +fn test_diff_cor_nbody() { + let (jpl_ephem, ut1_provider, obs_dataset, iod_params, _) = build_test_fixtures(); + + // Perturbers: Sun (central body) + Jupiter (dominant perturber in the + // main belt). Using the same rms_divergence_ratio as the 2-body test + // because the N-body corrector converges smoothly to its own minimum. + let nbody_config = NBodyConfig { + perturbing_bodies: vec![ + NaifIds::SSB(SolarSystemBary::Sun), + NaifIds::PB(PlanetaryBary::Jupiter), + ], + abs_tol: 1e-12, + rel_tol: 1e-12, + }; + + let diff_cor_config = DifferentialCorrectionConfig { + rms_divergence_ratio: 10.0, + propagator: PropagatorKind::NBody(nbody_config), + ..DifferentialCorrectionConfig::default() + }; + + let full_orbit = obs_dataset + .fit_lsq( + &jpl_ephem, + &ut1_provider, + ObsErrorModel::FCCT14, + &iod_params, + &diff_cor_config, + None, + &mut StdRng::seed_from_u64(42), + ) + .unwrap(); + + // 2-body oracle values (from test_diff_cor) used as reference below. + let twobody_sma_k09r05f = 1.801837227645679_f64; + let twobody_sma_33803 = 2.190614169340076_f64; + let twobody_sma_8467 = 3.2073734821020743_f64; + + // ------------------------------------------------------------------------- + // 2015 AB (K09R05F) — 5-year arc, larger N-body/2-body divergence expected + // ------------------------------------------------------------------------- + { + let orbit = full_orbit + .get(&TrajId::from("K09R05F")) + .expect("K09R05F not found in N-body results") + .as_ref() + .expect("K09R05F should converge under N-body propagation"); + + let elem = match orbit.orbital_elements() { + OrbitalElements::Equinoctial(e) => e, + _ => panic!("Expected equinoctial elements for K09R05F"), + }; + + // Semi-major axis must be positive and within 0.3 AU of the 2-body value. + assert!( + elem.semi_major_axis > 0.0, + "K09R05F N-body a must be positive" + ); + assert!( + (elem.semi_major_axis - twobody_sma_k09r05f).abs() < 0.3, + "K09R05F N-body a = {} differs from 2-body oracle {} by more than 0.3 AU", + elem.semi_major_axis, + twobody_sma_k09r05f + ); + + // Fit quality must remain physically reasonable. + assert!( + orbit.orbit_quality() < 5.0, + "K09R05F N-body orbit quality {} exceeds bound 5.0", + orbit.orbit_quality() + ); + } + + // ------------------------------------------------------------------------- + // 33803 — 5-month arc + // ------------------------------------------------------------------------- + { + let orbit = full_orbit + .get(&TrajId::Int(33803)) + .expect("33803 not found in N-body results") + .as_ref() + .expect("33803 should converge under N-body propagation"); + + let elem = match orbit.orbital_elements() { + OrbitalElements::Equinoctial(e) => e, + _ => panic!("Expected equinoctial elements for 33803"), + }; + + assert!( + elem.semi_major_axis > 0.0, + "33803 N-body a must be positive" + ); + assert!( + (elem.semi_major_axis - twobody_sma_33803).abs() < 0.1, + "33803 N-body a = {} differs from 2-body oracle {} by more than 0.1 AU", + elem.semi_major_axis, + twobody_sma_33803 + ); + assert!( + orbit.orbit_quality() < 5.0, + "33803 N-body orbit quality {} exceeds bound 5.0", + orbit.orbit_quality() + ); + } + + // ------------------------------------------------------------------------- + // 8467 — 40-day arc (tightest comparison with 2-body oracle) + // ------------------------------------------------------------------------- + { + let orbit = full_orbit + .get(&TrajId::Int(8467)) + .expect("8467 not found in N-body results") + .as_ref() + .expect("8467 should converge under N-body propagation"); + + let elem = match orbit.orbital_elements() { + OrbitalElements::Equinoctial(e) => e, + _ => panic!("Expected equinoctial elements for 8467"), + }; + + let twobody_8467 = EquinoctialElements { + reference_epoch: 60672.2443617134, + semi_major_axis: twobody_sma_8467, + eccentricity_sin_lon: 0.053597752212361474, + eccentricity_cos_lon: -0.023229330026225303, + tan_half_incl_sin_node: 0.0028890355813102732, + tan_half_incl_cos_node: 0.09179492536540514, + mean_longitude: 0.626741395885302, + }; + + // Over a 40-day arc at 3.2 AU, Jovian perturbations produce element + // changes well below 5e-2 in dimensionless units. + let a_tol = 5e-2; // AU + let e_tol = 5e-2; // eccentricity components (dimensionless) + + assert!( + (elem.semi_major_axis - twobody_8467.semi_major_axis).abs() < a_tol, + "8467 N-body a = {} differs from 2-body oracle {} by more than {} AU", + elem.semi_major_axis, + twobody_8467.semi_major_axis, + a_tol + ); + assert!( + (elem.eccentricity_sin_lon - twobody_8467.eccentricity_sin_lon).abs() < e_tol, + "8467 N-body h = {} differs from 2-body oracle {} by more than {}", + elem.eccentricity_sin_lon, + twobody_8467.eccentricity_sin_lon, + e_tol + ); + assert!( + (elem.eccentricity_cos_lon - twobody_8467.eccentricity_cos_lon).abs() < e_tol, + "8467 N-body k = {} differs from 2-body oracle {} by more than {}", + elem.eccentricity_cos_lon, + twobody_8467.eccentricity_cos_lon, + e_tol + ); + assert!( + orbit.orbit_quality() < 5.0, + "8467 N-body orbit quality {} exceeds bound 5.0", + orbit.orbit_quality() + ); + } +} + +/// Strict non-regression test for the N-body differential corrector. +/// +/// Reference values were captured from a deterministic run of `test_diff_cor_nbody` +/// with `--nocapture` and must remain reproducible to 1e-10. +#[test] +fn test_diff_cor_nbody_nonregression() { + let (jpl_ephem, ut1_provider, obs_dataset, iod_params, _) = build_test_fixtures(); + + let nbody_config = NBodyConfig { + perturbing_bodies: vec![ + NaifIds::SSB(SolarSystemBary::Sun), + NaifIds::PB(PlanetaryBary::Jupiter), + ], + abs_tol: 1e-12, + rel_tol: 1e-12, + }; + + let diff_cor_config = DifferentialCorrectionConfig { + rms_divergence_ratio: 10.0, + propagator: PropagatorKind::NBody(nbody_config), + ..DifferentialCorrectionConfig::default() + }; + + let full_orbit = obs_dataset + .fit_lsq( + &jpl_ephem, + &ut1_provider, + ObsErrorModel::FCCT14, + &iod_params, + &diff_cor_config, + None, + &mut StdRng::seed_from_u64(42), + ) + .unwrap(); + + let tol = 1e-10_f64; + + // ------------------------------------------------------------------------- + // 8467 — reference N-body final equinoctial elements + orbit quality + // ------------------------------------------------------------------------- + { + let orbit = full_orbit + .get(&TrajId::Int(8467)) + .expect("8467 not found") + .as_ref() + .expect("8467 should converge"); + + let expected = OrbitalElements::Equinoctial(EquinoctialElements { + reference_epoch: 60672.2443617134, + semi_major_axis: 3.2064058028481552, + eccentricity_sin_lon: 0.05300520970081429, + eccentricity_cos_lon: -0.023197692700634636, + tan_half_incl_sin_node: 0.002896813138792025, + tan_half_incl_cos_node: 0.09181010554057693, + mean_longitude: 0.6256995904459722, + }); + + assert!( + approx_equal(&expected, orbit.orbital_elements(), tol), + "8467 N-body orbital elements differ from oracle beyond tolerance {tol}" + ); + assert_relative_eq!(orbit.orbit_quality(), 0.3486122845933199, epsilon = tol); + } + + // ------------------------------------------------------------------------- + // 33803 — reference N-body final equinoctial elements + orbit quality + // ------------------------------------------------------------------------- + { + let orbit = full_orbit + .get(&TrajId::Int(33803)) + .expect("33803 not found") + .as_ref() + .expect("33803 should converge"); + + let expected = OrbitalElements::Equinoctial(EquinoctialElements { + reference_epoch: 60465.26777915681, + semi_major_axis: 2.190348311458185, + eccentricity_sin_lon: -0.13373910921857446, + eccentricity_cos_lon: 0.15339157238172804, + tan_half_incl_sin_node: 0.0029876412023201416, + tan_half_incl_cos_node: -0.05950692044872062, + mean_longitude: 4.224365041422834, + }); + + assert!( + approx_equal(&expected, orbit.orbital_elements(), tol), + "33803 N-body orbital elements differ from oracle beyond tolerance {tol}" + ); + assert_relative_eq!(orbit.orbit_quality(), 0.7034091187041202, epsilon = tol); + } + + // ------------------------------------------------------------------------- + // K09R05F — reference N-body final equinoctial elements + orbit quality + // ------------------------------------------------------------------------- + { + let orbit = full_orbit + .get(&TrajId::from("K09R05F")) + .expect("K09R05F not found") + .as_ref() + .expect("K09R05F should converge"); + + let expected = OrbitalElements::Equinoctial(EquinoctialElements { + reference_epoch: 57049.2684537375, + semi_major_axis: 1.8021517900042052, + eccentricity_sin_lon: 0.2694922786015968, + eccentricity_cos_lon: 0.08955282358108035, + tan_half_incl_sin_node: 0.0008974287327937245, + tan_half_incl_cos_node: 0.10167548786557225, + mean_longitude: 1.6921653421358704, + }); + + assert!( + approx_equal(&expected, orbit.orbital_elements(), tol), + "K09R05F N-body orbital elements differ from oracle beyond tolerance {tol}" + ); + assert_relative_eq!(orbit.orbit_quality(), 0.3608868439717083, epsilon = tol); + } +}