diff --git a/Cargo.toml b/Cargo.toml index 93bb562..4aae1b8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -30,3 +30,11 @@ approx = "0.5" serde_json = "1.0" criterion = "0.7" num-dual = "0.15" + +[[bench]] +name = "kepler" +harness = false + +[[bench]] +name = "propagation" +harness = false diff --git a/benches/kepler.rs b/benches/kepler.rs new file mode 100644 index 0000000..e50d9f6 --- /dev/null +++ b/benches/kepler.rs @@ -0,0 +1,128 @@ +//! Element conversions and the two-body relations. +//! +//! These run per measurement in a filter and per step in some force models, so a +//! slow conversion is not a rounding error in the budget. The point of tracking +//! them is to notice a regression, not to hit a number. + +use std::hint::black_box; + +use astrokit::kepler::{self, Degeneracy, Keplerian}; +use criterion::{criterion_group, criterion_main, Criterion}; +use nalgebra::Vector3; + +const MU: f64 = 3.986004415e14; + +/// Inclined, near-circular LEO. Just outside the degeneracy tolerance, so it +/// takes the general branch. +fn leo() -> (Vector3, Vector3) { + ( + Vector3::new( + 4.263868426884883e6, + 5.146189057155391e6, + 1.1310208421331816e6, + ), + Vector3::new(-5923.454461876975, 4496.802639690076, 1870.3893008991558), + ) +} + +/// GEO: circular and equatorial, so it takes the doubly-degenerate branch. +fn geo() -> (Vector3, Vector3) { + let radius = 4.2164e7; + ( + Vector3::new(radius, 0.0, 0.0), + Vector3::new(0.0, kepler::circular_speed(radius, MU), 0.0), + ) +} + +fn bench_scalar_relations(c: &mut Criterion) { + let mut group = c.benchmark_group("kepler/relations"); + let sma = 6.7781363e6; + + group.bench_function("period", |b| { + b.iter(|| kepler::period(black_box(sma), black_box(MU))) + }); + group.bench_function("semi_major_axis", |b| { + b.iter(|| kepler::semi_major_axis(black_box(5553.6), black_box(MU))) + }); + group.bench_function("mean_motion", |b| { + b.iter(|| kepler::mean_motion(black_box(sma), black_box(MU))) + }); + group.finish(); +} + +fn bench_anomalies(c: &mut Criterion) { + let mut group = c.benchmark_group("kepler/anomalies"); + + group.bench_function("eccentric_from_true", |b| { + b.iter(|| kepler::eccentric_from_true(black_box(1.2), black_box(0.3))) + }); + group.bench_function("mean_from_eccentric", |b| { + b.iter(|| kepler::mean_from_eccentric(black_box(1.1), black_box(0.3))) + }); + + // The only iterative routine here, and the one worth watching. Near-circular + // converges in a step or two; high eccentricity is the expensive case. + for eccentricity in [0.001, 0.3, 0.9] { + group.bench_function(format!("eccentric_from_mean/e={eccentricity}"), |b| { + b.iter(|| kepler::eccentric_from_mean(black_box(1.0), black_box(eccentricity))) + }); + } + group.finish(); +} + +fn bench_conversions(c: &mut Criterion) { + let mut group = c.benchmark_group("kepler/conversions"); + + let (r, v) = leo(); + group.bench_function("from_cartesian/leo", |b| { + b.iter(|| Keplerian::from_cartesian(black_box(r), black_box(v), black_box(MU))) + }); + + let (r_geo, v_geo) = geo(); + group.bench_function("from_cartesian/geo", |b| { + b.iter(|| Keplerian::from_cartesian(black_box(r_geo), black_box(v_geo), black_box(MU))) + }); + + let elements = Keplerian { + semi_major_axis: 2.0e7, + eccentricity: 0.3, + inclination: 0.8, + raan: 0.5, + aop: 1.0, + true_anomaly: 2.0, + degeneracy: Degeneracy::None, + }; + group.bench_function("to_cartesian", |b| { + b.iter(|| black_box(elements).to_cartesian(black_box(MU))) + }); + + group.bench_function("round_trip", |b| { + b.iter(|| { + let elements = Keplerian::from_cartesian(black_box(r), black_box(v), MU); + black_box(elements.to_cartesian(MU)) + }) + }); + group.finish(); +} + +fn bench_invariants(c: &mut Criterion) { + let mut group = c.benchmark_group("kepler/invariants"); + let (r, v) = leo(); + + group.bench_function("specific_energy", |b| { + b.iter(|| kepler::specific_energy(black_box(r), black_box(v), black_box(MU))) + }); + group.bench_function("specific_angular_momentum", |b| { + b.iter(|| kepler::specific_angular_momentum(black_box(r), black_box(v))) + }); + group.finish(); +} + +criterion_group!( + benches, + bench_scalar_relations, + bench_anomalies, + bench_conversions, + bench_invariants +); +criterion_main!(benches); diff --git a/benches/propagation.rs b/benches/propagation.rs new file mode 100644 index 0000000..c81dfe9 --- /dev/null +++ b/benches/propagation.rs @@ -0,0 +1,187 @@ +//! Two-body propagation, and the derivative evaluation underneath it. +//! +//! The derivative benchmarks are the ones that matter most: a propagation +//! evaluates them thousands of times per day of arc, so anything wasteful in +//! assembly or in a model shows up here before it shows up in a wall-clock +//! figure. They also isolate the cost of a layout, which should be nil — the +//! `Orbit`, `OrbitMass` and `Full` numbers ought to differ only by the work of +//! moving more elements around. + +use std::hint::black_box; + +use astrokit::dynamics::{self, Context}; +use astrokit::kepler; +use astrokit::prelude::*; +use astrokit::propagation::{state_derivative, Assembly}; +use criterion::{criterion_group, criterion_main, Criterion}; +use nalgebra::Vector3; + +const MU: f64 = 3.986004415e14; +const SMA: f64 = 6.7781363e6; + +fn epoch() -> Epoch { + Epoch::from_gregorian_utc_hms(2026, 1, 1, 0, 0, 0) +} + +fn position() -> Vector3 { + Vector3::new( + 4.263868426884883e6, + 5.146189057155391e6, + 1.1310208421331816e6, + ) +} + +fn velocity() -> Vector3 { + Vector3::new(-5923.454461876975, 4496.802639690076, 1870.3893008991558) +} + +fn two_body() -> Vec { + vec![TwoBody { mu: MU }] +} + +/// One derivative evaluation, per layout. The layout should cost nothing. +fn bench_derivative(c: &mut Criterion) { + let mut group = c.benchmark_group("derivative"); + + let assembly = Assembly::new( + dynamics::build::(&two_body()), + Context::new(epoch()), + ); + let y = Orbit.with_position(position()).with_velocity(velocity()); + group.bench_function("two_body/Orbit", |b| { + b.iter(|| state_derivative(black_box(0.0), black_box(y.vector), &assembly)) + }); + + let assembly = Assembly::new( + dynamics::build::(&two_body()), + Context::new(epoch()), + ); + let y = OrbitMass + .with_position(position()) + .with_velocity(velocity()) + .with_mass(500.0); + group.bench_function("two_body/OrbitMass", |b| { + b.iter(|| state_derivative(black_box(0.0), black_box(y.vector), &assembly)) + }); + + let assembly = Assembly::new( + dynamics::build::(&two_body()), + Context::new(epoch()), + ); + let y = Full + .with_position(position()) + .with_velocity(velocity()) + .with_mass(500.0); + group.bench_function("two_body/Full", |b| { + b.iter(|| state_derivative(black_box(0.0), black_box(y.vector), &assembly)) + }); + + group.finish(); +} + +/// A full propagation at each accuracy preset, over one day. +fn bench_propagation(c: &mut Criterion) { + let mut group = c.benchmark_group("propagation/one_day"); + group.sample_size(20); + + let forces = two_body(); + for accuracy in [Accuracy::Low, Accuracy::Medium, Accuracy::High] { + group.bench_function(format!("{accuracy:?}"), |b| { + b.iter(|| { + let y0 = Orbit.with_position(position()).with_velocity(velocity()); + let mut propagation = Propagation::new(epoch(), y0, &forces) + .duration(Duration::from_seconds(86400.0)) + .accuracy(accuracy); + black_box(propagation.run().states.len()) + }) + }); + } + group.finish(); +} + +/// One orbital period, which is the unit most of the tests work in. +fn bench_one_period(c: &mut Criterion) { + let forces = two_body(); + let period = Duration::from_seconds(kepler::period(SMA, MU)); + + c.bench_function("propagation/one_period", |b| { + b.iter(|| { + let y0 = Orbit.with_position(position()).with_velocity(velocity()); + let mut propagation = Propagation::new(epoch(), y0, &forces) + .duration(period) + .accuracy(Accuracy::Medium); + black_box(propagation.run().states.len()) + }) + }); +} + +/// The cost of a state transition matrix relative to a plain propagation. +/// +/// Stage 1 measured 10.6x for a 6x6 on a two-body arc. Central differences would +/// need thirteen propagations and lose about half the digits, so anything near +/// that ratio is a win; this tracks it as the force models get heavier. +#[cfg(feature = "autodiff")] +fn bench_state_transition_matrix(c: &mut Criterion) { + use nalgebra::{Matrix6, Vector6}; + use num_dual::DualSVec64; + + let mut group = c.benchmark_group("propagation/stm"); + group.sample_size(20); + + let forces = two_body(); + let period = Duration::from_seconds(kepler::period(SMA, MU)); + let state = Vector6::new( + position()[0], + position()[1], + position()[2], + velocity()[0], + velocity()[1], + velocity()[2], + ); + + group.bench_function("plain", |b| { + b.iter(|| { + let y0 = Orbit.state(state); + let mut propagation = Propagation::new(epoch(), y0, &forces) + .duration(period) + .accuracy(Accuracy::Medium); + black_box(*propagation.run().states.last().unwrap()) + }) + }); + + group.bench_function("with_stm", |b| { + b.iter(|| { + let (_, stm): (Vector6, Matrix6) = num_dual::jacobian( + |y: Vector6>| { + let mut propagation = Propagation::new(epoch(), Orbit.state(y), &forces) + .duration(period) + .accuracy(Accuracy::Medium); + *propagation.run().states.last().unwrap() + }, + &state, + ); + black_box(stm) + }) + }); + + group.finish(); +} + +#[cfg(feature = "autodiff")] +criterion_group!( + benches, + bench_derivative, + bench_propagation, + bench_one_period, + bench_state_transition_matrix +); + +#[cfg(not(feature = "autodiff"))] +criterion_group!( + benches, + bench_derivative, + bench_propagation, + bench_one_period +); + +criterion_main!(benches); diff --git a/examples/two_body.rs b/examples/two_body.rs index dc97777..cc7ec25 100644 --- a/examples/two_body.rs +++ b/examples/two_body.rs @@ -2,9 +2,9 @@ //! //! Run with `cargo run --example two_body`. +use astrokit::kepler; use astrokit::prelude::*; use nalgebra::Vector3; -use std::f64::consts::PI; /// Earth's gravitational parameter, m^3/s^2. const MU: f64 = 3.986004415e14; @@ -17,7 +17,7 @@ const EARTH_RADIUS: f64 = 6.378137e6; fn main() { let epoch = Epoch::from_gregorian_utc_hms(2026, 1, 1, 0, 0, 0); - let period = Duration::from_seconds(2.0 * PI * (SMA.powi(3) / MU).sqrt()); + let period = Duration::from_seconds(kepler::period(SMA, MU)); let y0 = Orbit .with_position(Vector3::new( @@ -40,8 +40,16 @@ fn main() { let end = Orbit.state(*solution.states.last().unwrap()); + let elements = y0.keplerian(MU); println!("epoch: {epoch}"); println!("period: {period}"); + println!( + "elements: a = {:.3} km, e = {:.6}, i = {:.3} deg ({:?})", + elements.semi_major_axis / 1e3, + elements.eccentricity, + elements.inclination.to_degrees(), + elements.degeneracy + ); println!("steps: {}", solution.states.len() - 1); println!( "position error: {:.3e} m", @@ -58,4 +66,8 @@ fn main() { "altitude at half a period: {:.3} km", (half.position().norm() - EARTH_RADIUS) / 1e3 ); + println!( + "specific energy drift: {:.3e} J/kg", + end.specific_energy(MU) - y0.specific_energy(MU) + ); } diff --git a/src/kepler.rs b/src/kepler.rs new file mode 100644 index 0000000..b5282b5 --- /dev/null +++ b/src/kepler.rs @@ -0,0 +1,718 @@ +//! Keplerian two-body relations. +//! +//! Everything here is generic over the scalar, so it can be differentiated. That +//! is the reason these exist at all rather than deferring to [`anise`], whose +//! equivalents are `f64` only — a filter needs exact partials of, say, a +//! semi-major axis with respect to the state. ANISE's versions are used as an +//! independent cross-check in the tests. +//! +//! `mu` stays `f64` throughout. Derivatives with respect to a gravitational +//! parameter are not something anything here wants, and keeping it real avoids +//! lifting it at every call site. + +use core::f64::consts::TAU; + +use nalgebra::Vector3; +use ordinary_diffeq::scalar::Scalar; + +use crate::state::{HasPosition, HasVelocity, State}; + +/// How close to zero an eccentricity or inclination has to be before +/// [`Keplerian::from_cartesian`] reports a [`Degeneracy`] and substitutes. +/// +/// Deliberately tight. A looser threshold would discard the argument of periapsis +/// of an orbit that has a small but real eccentricity, which is worse than +/// returning an ill-conditioned angle the caller can see is ill-conditioned. Note +/// that an element set just outside this tolerance is *defined* but sensitive: at +/// `e = 1e-6` the argument of periapsis is real and almost arbitrary. +pub const DEGENERACY_TOLERANCE: f64 = 1e-11; + +/// Orbital period, in seconds. +pub fn period(semi_major_axis: T, mu: f64) -> T { + (semi_major_axis.powi(3) / mu).sqrt() * TAU +} + +/// Semi-major axis, in meters, of an orbit with this period in seconds. +pub fn semi_major_axis(period: T, mu: f64) -> T { + ((period / TAU).powi(2) * mu).cbrt() +} + +/// Mean motion, in radians per second. +pub fn mean_motion(semi_major_axis: T, mu: f64) -> T { + (T::lift(mu) / semi_major_axis.powi(3)).sqrt() +} + +/// Circular orbit speed at radius `radius`, in meters per second. +pub fn circular_speed(radius: T, mu: f64) -> T { + (T::lift(mu) / radius).sqrt() +} + +/// Specific orbital energy, in J/kg. +/// +/// Conserved under two-body motion, which makes it a check on a propagation that +/// is independent of the integrator's own error estimate. +pub fn specific_energy(position: Vector3, velocity: Vector3, mu: f64) -> T { + velocity.norm_squared() * 0.5 - T::lift(mu) / position.norm() +} + +/// Specific angular momentum, in m^2/s. Also conserved under two-body motion. +pub fn specific_angular_momentum( + position: Vector3, + velocity: Vector3, +) -> Vector3 { + position.cross(&velocity) +} + +/// Eccentric anomaly from true anomaly, both in radians. +pub fn eccentric_from_true(true_anomaly: T, eccentricity: T) -> T { + let one = T::lift(1.0); + let factor = ((one - eccentricity) / (one + eccentricity)).sqrt(); + (factor * (true_anomaly * 0.5).tan()).atan() * 2.0 +} + +/// True anomaly from eccentric anomaly, both in radians. +pub fn true_from_eccentric(eccentric_anomaly: T, eccentricity: T) -> T { + let one = T::lift(1.0); + let factor = ((one + eccentricity) / (one - eccentricity)).sqrt(); + (factor * (eccentric_anomaly * 0.5).tan()).atan() * 2.0 +} + +/// Mean anomaly from eccentric anomaly, both in radians. Kepler's equation, +/// forward, which is the direction that is closed-form. +pub fn mean_from_eccentric(eccentric_anomaly: T, eccentricity: T) -> T { + eccentric_anomaly - eccentricity * eccentric_anomaly.sin() +} + +/// Eccentric anomaly from mean anomaly, both in radians. +/// +/// Kepler's equation inverted by Newton iteration. Under a dual scalar the +/// derivative converges alongside the value, because each iterate is an +/// arithmetic function of the last — no special handling is needed, and the +/// result is the exact derivative of the converged iterate. +pub fn eccentric_from_mean(mean_anomaly: T, eccentricity: T) -> T { + let one = T::lift(1.0); + // Starting from M + e sin M rather than M saves an iteration or two at + // moderate eccentricity. + let mut eccentric = mean_anomaly + eccentricity * mean_anomaly.sin(); + for _ in 0..50 { + let residual = eccentric - eccentricity * eccentric.sin() - mean_anomaly; + if residual.re().abs() < 1e-14 { + break; + } + eccentric -= residual / (one - eccentricity * eccentric.cos()); + } + eccentric +} + +/// The angle from `from` to `to`, measured about `unit_normal`, in `[0, 2pi)`. +/// +/// `atan2` of the cross and dot products, rather than `acos` of a normalized dot +/// product followed by a sign correction. Three reasons: `acos` loses precision +/// near zero and pi, which is exactly where near-circular and near-equatorial +/// orbits sit; its argument has to be clamped in case rounding pushes it out of +/// domain; and recovering which half of the revolution the angle lies in needs a +/// branch, which this does not. +fn angle_about(from: Vector3, to: Vector3, unit_normal: Vector3) -> T { + // (a x b) . n = |a||b| sin(theta) for a unit n normal to both, and + // a . b = |a||b| cos(theta), so the magnitudes cancel in the atan2. + let sine = from.cross(&to).dot(&unit_normal); + let cosine = from.dot(&to); + wrap_two_pi(sine.atan2(cosine)) +} + +/// Wraps an angle into `[0, 2pi)`. +fn wrap_two_pi(angle: T) -> T { + let turns = (angle.re() / TAU).floor(); + angle - TAU * turns +} + +/// Classical Keplerian orbital elements. +/// +/// Angles are radians and lengths are meters, like everything else here. +/// +/// Three element sets are singular, and the conventional substitutions are used +/// rather than returning a NaN. Which one applies is recorded in +/// [`Keplerian::degeneracy`], because a caller reading `aop` needs to know +/// whether it means what it says: +/// +/// - **circular and inclined** — periapsis is undefined, so `aop` is zero and +/// `true_anomaly` carries the argument of latitude +/// - **equatorial and elliptical** — the node is undefined, so `raan` is zero and +/// `aop` carries the longitude of periapsis +/// - **circular and equatorial** — both are undefined, so `raan` and `aop` are +/// zero and `true_anomaly` carries the true longitude +/// +/// GEO hits the third case and near-circular LEO the first, so these are the +/// common cases here rather than corner cases. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Keplerian { + /// Semi-major axis, m. + pub semi_major_axis: T, + /// Eccentricity, dimensionless. + pub eccentricity: T, + /// Inclination, rad. + pub inclination: T, + /// Right ascension of the ascending node, rad. + pub raan: T, + /// Argument of periapsis, rad. + pub aop: T, + /// True anomaly, rad. + pub true_anomaly: T, + /// Which elements, if any, are not what they are named. + pub degeneracy: Degeneracy, +} + +/// Which of a [`Keplerian`] set's angles are degenerate. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum Degeneracy { + /// Every element means what it is named. + None, + /// Circular and inclined: `true_anomaly` is the argument of latitude. + Circular, + /// Equatorial and elliptical: `aop` is the longitude of periapsis. + Equatorial, + /// Circular and equatorial: `true_anomaly` is the true longitude. + CircularEquatorial, +} + +impl Keplerian { + /// Elements from an inertial position and velocity. + /// + /// Vallado's RV2COE, with the three singular cases handled as + /// [`Keplerian`] documents. + pub fn from_cartesian(position: Vector3, velocity: Vector3, mu: f64) -> Self { + let zero = T::lift(0.0); + let r = position.norm(); + + let momentum = position.cross(&velocity); + let node = Vector3::new(-momentum[1], momentum[0], zero); + + // e = ((v^2 - mu/r) r - (r . v) v) / mu + let eccentricity_vector = (position * (velocity.norm_squared() - T::lift(mu) / r) + - velocity * position.dot(&velocity)) + / T::lift(mu); + let eccentricity = eccentricity_vector.norm(); + + let energy = specific_energy(position, velocity, mu); + let semi_major_axis = -T::lift(mu) / (energy * 2.0); + + // atan2 of the in-plane and out-of-plane components of the angular + // momentum, which is well conditioned at zero and pi where an acos of a + // ratio is not. + let inclination = (momentum[0].powi(2) + momentum[1].powi(2)) + .sqrt() + .atan2(momentum[2]); + let retrograde = momentum[2].re() < 0.0; + + let circular = eccentricity.re() < DEGENERACY_TOLERANCE; + let equatorial = inclination.re() < DEGENERACY_TOLERANCE + || (inclination.re() - core::f64::consts::PI).abs() < DEGENERACY_TOLERANCE; + + // A closure, not a binding: the two equatorial branches discard it, and + // there is no sense computing an atan2 to throw it away. + let raan = || wrap_two_pi(node[1].atan2(node[0])); + + let (raan, aop, true_anomaly, degeneracy) = match (circular, equatorial) { + (true, true) => { + // True longitude, in the equatorial plane. Retrograde runs the + // other way round. + let longitude = position[1].atan2(position[0]); + let longitude = if retrograde { -longitude } else { longitude }; + ( + zero, + zero, + wrap_two_pi(longitude), + Degeneracy::CircularEquatorial, + ) + } + (true, false) => ( + raan(), + zero, + // Argument of latitude: node to position, in the orbital plane. + angle_about(node, position, momentum.normalize()), + Degeneracy::Circular, + ), + (false, true) => { + // Longitude of periapsis, in the equatorial plane. + let longitude = eccentricity_vector[1].atan2(eccentricity_vector[0]); + let longitude = if retrograde { -longitude } else { longitude }; + ( + zero, + wrap_two_pi(longitude), + angle_about(eccentricity_vector, position, momentum.normalize()), + Degeneracy::Equatorial, + ) + } + (false, false) => { + // Only the branches that measure an in-plane angle need this, and + // the circular-equatorial one does not: computing it up front cost + // GEO 13% for nothing. + let orbit_normal = momentum.normalize(); + ( + raan(), + // Node to periapsis, in the orbital plane. + angle_about(node, eccentricity_vector, orbit_normal), + angle_about(eccentricity_vector, position, orbit_normal), + Degeneracy::None, + ) + } + }; + + Self { + semi_major_axis, + eccentricity, + inclination, + raan, + aop, + true_anomaly, + degeneracy, + } + } + + /// An inertial position and velocity, in meters and meters per second. + /// + /// Inverts [`Keplerian::from_cartesian`] for a non-degenerate set. A + /// degenerate set round-trips too, because the substituted angles are zero + /// where they are undefined and the perifocal construction then puts the + /// argument of latitude or true longitude in the right place. + pub fn to_cartesian(&self, mu: f64) -> (Vector3, Vector3) { + let one = T::lift(1.0); + let semi_latus = self.semi_major_axis * (one - self.eccentricity.powi(2)); + let radius = semi_latus / (one + self.eccentricity * self.true_anomaly.cos()); + + // Perifocal frame, then rotated out by aop, inclination and raan. + let in_plane = Vector3::new( + radius * self.true_anomaly.cos(), + radius * self.true_anomaly.sin(), + T::lift(0.0), + ); + let speed = (T::lift(mu) / semi_latus).sqrt(); + let velocity_in_plane = Vector3::new( + -self.true_anomaly.sin() * speed, + (self.eccentricity + self.true_anomaly.cos()) * speed, + T::lift(0.0), + ); + + ( + self.perifocal_to_inertial(in_plane), + self.perifocal_to_inertial(velocity_in_plane), + ) + } + + /// Rotates a perifocal vector into the inertial frame, by aop, inclination + /// and right ascension in that order. + fn perifocal_to_inertial(&self, v: Vector3) -> Vector3 { + let (sin_aop, cos_aop) = (self.aop.sin(), self.aop.cos()); + let (sin_inc, cos_inc) = (self.inclination.sin(), self.inclination.cos()); + let (sin_raan, cos_raan) = (self.raan.sin(), self.raan.cos()); + + // Rotated by aop about z. + let x = v[0] * cos_aop - v[1] * sin_aop; + let y = v[0] * sin_aop + v[1] * cos_aop; + let z = v[2]; + + // Then by inclination about x. + let (y, z) = (y * cos_inc - z * sin_inc, y * sin_inc + z * cos_inc); + + // Then by raan about z. + Vector3::new(x * cos_raan - y * sin_raan, x * sin_raan + y * cos_raan, z) + } + + /// Orbital period, in seconds. + pub fn period(&self, mu: f64) -> T { + period(self.semi_major_axis, mu) + } + + /// Mean motion, in radians per second. + pub fn mean_motion(&self, mu: f64) -> T { + mean_motion(self.semi_major_axis, mu) + } + + /// Eccentric anomaly, in radians. + pub fn eccentric_anomaly(&self) -> T { + eccentric_from_true(self.true_anomaly, self.eccentricity) + } + + /// Mean anomaly, in radians. + pub fn mean_anomaly(&self) -> T { + mean_from_eccentric(self.eccentric_anomaly(), self.eccentricity) + } + + /// Periapsis radius, in meters. + pub fn periapsis(&self) -> T { + self.semi_major_axis * (T::lift(1.0) - self.eccentricity) + } + + /// Apoapsis radius, in meters. + pub fn apoapsis(&self) -> T { + self.semi_major_axis * (T::lift(1.0) + self.eccentricity) + } +} + +/// Layouts carrying a full translational state. +/// +/// Blanket-implemented, so any layout with both a position and a velocity gets +/// these without doing anything. +pub trait TranslationalLayout: HasPosition + HasVelocity { + /// Starts a [`State`] from Keplerian elements. + fn with_keplerian(self, elements: Keplerian, mu: f64) -> State { + let (position, velocity) = elements.to_cartesian(mu); + self.with_position(position).with_velocity(velocity) + } +} + +impl + HasVelocity> TranslationalLayout for L {} + +impl + HasVelocity> State { + /// The Keplerian elements of this state, about a body of parameter `mu`. + pub fn keplerian(&self, mu: f64) -> Keplerian { + Keplerian::from_cartesian(self.position(), self.velocity(), mu) + } + + /// Orbital period, in seconds. + pub fn period(&self, mu: f64) -> T { + period(self.keplerian(mu).semi_major_axis, mu) + } + + /// Specific orbital energy, in J/kg. + pub fn specific_energy(&self, mu: f64) -> T { + specific_energy(self.position(), self.velocity(), mu) + } + + /// Specific angular momentum, in m^2/s. + pub fn specific_angular_momentum(&self) -> Vector3 { + specific_angular_momentum(self.position(), self.velocity()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::state::Orbit; + use approx::assert_relative_eq; + use core::f64::consts::PI; + + const MU: f64 = 3.986004415e14; + + /// The state the propagation tests use, an inclined near-circular LEO. + fn leo() -> (Vector3, Vector3) { + ( + Vector3::new( + 4.263868426884883e6, + 5.146189057155391e6, + 1.1310208421331816e6, + ), + Vector3::new(-5923.454461876975, 4496.802639690076, 1870.3893008991558), + ) + } + + #[test] + fn test_period_and_semi_major_axis_invert_each_other() { + let sma = 6.7781363e6; + let p = period(sma, MU); + assert_relative_eq!(p, 5553.623413031, max_relative = 1e-9); + assert_relative_eq!(semi_major_axis(p, MU), sma, max_relative = 1e-12); + assert_relative_eq!(mean_motion(sma, MU), TAU / p, max_relative = 1e-12); + } + + #[test] + fn test_circular_speed_gives_a_closed_orbit() { + let radius = 7.0e6; + let speed = circular_speed(radius, MU); + let elements = Keplerian::from_cartesian( + Vector3::new(radius, 0.0, 0.0), + Vector3::new(0.0, speed, 0.0), + MU, + ); + assert_relative_eq!(elements.semi_major_axis, radius, max_relative = 1e-12); + assert!(elements.eccentricity < 1e-15); + } + + #[test] + fn test_cartesian_round_trips_through_elements() { + let (r, v) = leo(); + let elements = Keplerian::from_cartesian(r, v, MU); + let (r2, v2) = elements.to_cartesian(MU); + assert_relative_eq!(r2, r, max_relative = 1e-12); + assert_relative_eq!(v2, v, max_relative = 1e-12); + } + + /// An inclined elliptical orbit, where no element is degenerate. + #[test] + fn test_non_degenerate_elements_round_trip() { + let elements = Keplerian { + semi_major_axis: 2.0e7, + eccentricity: 0.3, + inclination: 45.0_f64.to_radians(), + raan: 30.0_f64.to_radians(), + aop: 60.0_f64.to_radians(), + true_anomaly: 120.0_f64.to_radians(), + degeneracy: Degeneracy::None, + }; + let (r, v) = elements.to_cartesian(MU); + let back = Keplerian::from_cartesian(r, v, MU); + + assert_eq!(back.degeneracy, Degeneracy::None); + assert_relative_eq!( + back.semi_major_axis, + elements.semi_major_axis, + max_relative = 1e-12 + ); + assert_relative_eq!( + back.eccentricity, + elements.eccentricity, + max_relative = 1e-12 + ); + assert_relative_eq!(back.inclination, elements.inclination, max_relative = 1e-12); + assert_relative_eq!(back.raan, elements.raan, max_relative = 1e-12); + assert_relative_eq!(back.aop, elements.aop, max_relative = 1e-12); + assert_relative_eq!( + back.true_anomaly, + elements.true_anomaly, + max_relative = 1e-12 + ); + } + + /// GEO is circular and equatorial, the doubly-singular case, and it is one of + /// the study's scenarios rather than a corner case. + #[test] + fn test_geo_is_reported_as_circular_equatorial() { + let radius = 4.2164e7; + let elements = Keplerian::from_cartesian( + Vector3::new(radius, 0.0, 0.0), + Vector3::new(0.0, circular_speed(radius, MU), 0.0), + MU, + ); + + assert_eq!(elements.degeneracy, Degeneracy::CircularEquatorial); + assert_relative_eq!(elements.semi_major_axis, radius, max_relative = 1e-12); + assert_eq!(elements.raan, 0.0); + assert_eq!(elements.aop, 0.0); + // The true longitude, which is zero along +x. + assert_relative_eq!(elements.true_anomaly, 0.0, epsilon = 1e-12); + + // A quarter turn later the true longitude is 90 degrees. + let quarter = Keplerian::from_cartesian( + Vector3::new(0.0, radius, 0.0), + Vector3::new(-circular_speed(radius, MU), 0.0, 0.0), + MU, + ); + assert_relative_eq!(quarter.true_anomaly, PI / 2.0, max_relative = 1e-12); + } + + /// A circular inclined orbit reports the argument of latitude. + #[test] + fn test_circular_inclined_reports_argument_of_latitude() { + let radius = 7.0e6; + let speed = circular_speed(radius, MU); + // Ascending node along +x, so at +x the argument of latitude is zero. + let elements = Keplerian::from_cartesian( + Vector3::new(radius, 0.0, 0.0), + Vector3::new(0.0, speed * 0.5, speed * 3.0_f64.sqrt() / 2.0), + MU, + ); + + assert_eq!(elements.degeneracy, Degeneracy::Circular); + assert_relative_eq!( + elements.inclination, + 60.0_f64.to_radians(), + max_relative = 1e-12 + ); + assert_eq!(elements.aop, 0.0); + assert_relative_eq!(elements.true_anomaly, 0.0, epsilon = 1e-12); + } + + #[test] + fn test_anomalies_round_trip() { + let eccentricity = 0.3; + for degrees in [0.0_f64, 30.0, 90.0, 150.0, 210.0, 330.0] { + let true_anomaly = degrees.to_radians(); + let eccentric = eccentric_from_true(true_anomaly, eccentricity); + assert_relative_eq!( + wrap_two_pi(true_from_eccentric(eccentric, eccentricity)), + wrap_two_pi(true_anomaly), + max_relative = 1e-12, + epsilon = 1e-12 + ); + + let mean = mean_from_eccentric(eccentric, eccentricity); + assert_relative_eq!( + eccentric_from_mean(mean, eccentricity), + eccentric, + max_relative = 1e-12, + epsilon = 1e-12 + ); + } + } + + #[test] + fn test_apsides_and_anomalies_on_an_ellipse() { + let elements = Keplerian { + semi_major_axis: 2.0e7, + eccentricity: 0.3, + inclination: 0.5, + raan: 0.0, + aop: 0.0, + true_anomaly: 0.0, + degeneracy: Degeneracy::None, + }; + assert_relative_eq!(elements.periapsis(), 1.4e7, max_relative = 1e-12); + assert_relative_eq!(elements.apoapsis(), 2.6e7, max_relative = 1e-12); + // At periapsis every anomaly is zero. + assert_eq!(elements.eccentric_anomaly(), 0.0); + assert_eq!(elements.mean_anomaly(), 0.0); + } + + #[test] + fn test_state_accessors() { + let (r, v) = leo(); + let y = Orbit.with_position(r).with_velocity(v); + + let elements = y.keplerian(MU); + assert_relative_eq!( + y.period(MU), + period(elements.semi_major_axis, MU), + max_relative = 1e-12 + ); + assert_relative_eq!( + y.specific_energy(MU), + specific_energy(r, v, MU), + max_relative = 1e-12 + ); + assert_eq!(y.specific_angular_momentum(), r.cross(&v)); + } + + #[test] + fn test_state_from_keplerian() { + let elements = Keplerian { + semi_major_axis: 7.0e6, + eccentricity: 0.01, + inclination: 0.9, + raan: 0.3, + aop: 0.6, + true_anomaly: 1.2, + degeneracy: Degeneracy::None, + }; + let y = Orbit.with_keplerian(elements, MU); + let back = y.keplerian(MU); + assert_relative_eq!( + back.semi_major_axis, + elements.semi_major_axis, + max_relative = 1e-12 + ); + assert_relative_eq!( + back.true_anomaly, + elements.true_anomaly, + max_relative = 1e-12 + ); + } + + /// Cross-check against ANISE, which is validated against SPICE. ANISE works + /// in kilometers and degrees, so the comparison is also a check that the unit + /// boundary is being crossed correctly. + #[test] + fn test_elements_agree_with_anise() { + use anise::constants::frames::EARTH_J2000; + use anise::prelude::Orbit as AniseOrbit; + use hifitime::Epoch; + + let (r, v) = leo(); + let ours = Keplerian::from_cartesian(r, v, MU); + + let frame = EARTH_J2000.with_mu_km3_s2(MU * 1e-9); + let theirs = AniseOrbit::new( + r[0] * 1e-3, + r[1] * 1e-3, + r[2] * 1e-3, + v[0] * 1e-3, + v[1] * 1e-3, + v[2] * 1e-3, + Epoch::from_gregorian_utc_hms(2026, 1, 1, 0, 0, 0), + frame, + ); + + assert_relative_eq!( + ours.semi_major_axis, + theirs.sma_km().unwrap() * 1e3, + max_relative = 1e-9 + ); + assert_relative_eq!( + ours.eccentricity, + theirs.ecc().unwrap(), + max_relative = 1e-9 + ); + assert_relative_eq!( + ours.inclination.to_degrees(), + theirs.inc_deg().unwrap(), + max_relative = 1e-9 + ); + assert_relative_eq!( + ours.raan.to_degrees(), + theirs.raan_deg().unwrap(), + max_relative = 1e-9 + ); + assert_relative_eq!( + ours.aop.to_degrees(), + theirs.aop_deg().unwrap(), + max_relative = 1e-9 + ); + assert_relative_eq!( + ours.true_anomaly.to_degrees(), + theirs.ta_deg().unwrap(), + max_relative = 1e-9 + ); + assert_relative_eq!( + period(ours.semi_major_axis, MU), + theirs.period().unwrap().to_seconds(), + max_relative = 1e-9 + ); + } + + /// The reason these are generic: a filter needs exact partials of an element + /// with respect to the state, and one dual propagation gives the whole row. + #[cfg(feature = "autodiff")] + #[test] + fn test_elements_differentiate() { + use nalgebra::{SVector, Vector6}; + use num_dual::DualSVec64; + + let (r, v) = leo(); + let state = Vector6::new(r[0], r[1], r[2], v[0], v[1], v[2]); + + // d(sma)/d(state), by automatic differentiation. + let (sma, gradient): (f64, SVector) = num_dual::gradient( + |y: Vector6>| { + let position = y.fixed_rows::<3>(0).into_owned(); + let velocity = y.fixed_rows::<3>(3).into_owned(); + Keplerian::from_cartesian(position, velocity, MU).semi_major_axis + }, + &state, + ); + + assert_relative_eq!( + sma, + Keplerian::from_cartesian(r, v, MU).semi_major_axis, + max_relative = 1e-12 + ); + + // Cross-check the gradient against central differences, which agree only + // to about the square root of machine epsilon. + let sma_of = |y: Vector6| { + Keplerian::from_cartesian( + y.fixed_rows::<3>(0).into_owned(), + y.fixed_rows::<3>(3).into_owned(), + MU, + ) + .semi_major_axis + }; + for i in 0..6 { + let h = 1e-5 * state[i].abs(); + let mut plus = state; + let mut minus = state; + plus[i] += h; + minus[i] -= h; + let difference = (sma_of(plus) - sma_of(minus)) / (2.0 * h); + assert_relative_eq!(gradient[i], difference, max_relative = 1e-6); + } + } +} diff --git a/src/lib.rs b/src/lib.rs index 6fb15f1..78f623b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -10,6 +10,7 @@ #![warn(missing_docs)] pub mod dynamics; +pub mod kepler; pub mod propagation; pub mod state; @@ -19,6 +20,7 @@ pub mod prelude { // The variants, so a script writes `TwoBody { mu }` rather than // `Dynamics::TwoBody { mu }`. Note this puts every model's name in scope. pub use crate::dynamics::Dynamics::*; + pub use crate::kepler::{Degeneracy, Keplerian, TranslationalLayout}; pub use crate::propagation::{Accuracy, Propagation}; pub use crate::state::{ Full, HasAngularRate, HasAttitude, HasEnergy, HasMass, HasPosition, HasVelocity, Layout, diff --git a/src/propagation.rs b/src/propagation.rs index c2a5fa9..36d5dd2 100644 --- a/src/propagation.rs +++ b/src/propagation.rs @@ -88,6 +88,14 @@ pub struct Assembly> { } impl> Assembly { + /// An assembly over an already-built model list. + /// + /// [`Propagation`] builds this itself; it is public so that a derivative can + /// be evaluated on its own, for a benchmark or a test. + pub fn new(models: Vec>>, context: Context) -> Self { + Self { models, context } + } + /// The names of the models in the list, in order. pub fn names(&self) -> Vec<&'static str> { self.models.iter().map(|m| m.name()).collect() @@ -553,12 +561,12 @@ mod tests { #[test] fn test_force_is_divided_by_the_mass() { - let assembly = Assembly:: { - models: vec![Box::new(ConstantForce { + let assembly: Assembly = Assembly::new( + vec![Box::new(ConstantForce { force: Vector3::new(10.0, 0.0, 0.0), })], - context: Context::new(epoch()), - }; + Context::new(epoch()), + ); let y = OrbitMass .with_position(leo().position()) @@ -574,15 +582,15 @@ mod tests { /// Force and acceleration are separate channels and both reach `dv/dt`. #[test] fn test_force_and_acceleration_both_contribute() { - let assembly = Assembly:: { - models: vec![ + let assembly: Assembly = Assembly::new( + vec![ Box::new(ConstantForce { force: Vector3::new(10.0, 0.0, 0.0), }), Dynamics::TwoBody { mu: MU }.build(), ], - context: Context::new(epoch()), - }; + Context::new(epoch()), + ); let y = OrbitMass .with_position(leo().position()) @@ -600,12 +608,12 @@ mod tests { #[test] #[should_panic(expected = "has no mass")] fn test_force_without_a_mass_component_is_caught() { - let assembly = Assembly:: { - models: vec![Box::new(ConstantForce { + let assembly: Assembly = Assembly::new( + vec![Box::new(ConstantForce { force: Vector3::new(10.0, 0.0, 0.0), })], - context: Context::new(epoch()), - }; + Context::new(epoch()), + ); let _ = state_derivative(0.0, leo().vector, &assembly); }