Some initial benchmarks and utilities, including keplerian elements. More typed coordinate stuff to come

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

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<f64>, Vector3<f64>) {
+    (
+        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<f64>, Vector3<f64>) {
+    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);

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<f64> {
+    Vector3::new(
+        4.263868426884883e6,
+        5.146189057155391e6,
+        1.1310208421331816e6,
+    )
+}
+
+fn velocity() -> Vector3<f64> {
+    Vector3::new(-5923.454461876975, 4496.802639690076, 1870.3893008991558)
+}
+
+fn two_body() -> Vec<Dynamics> {
+    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::<f64, 6, Orbit>(&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::<f64, 7, OrbitMass>(&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::<f64, 14, Full>(&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<f64>, Matrix6<f64>) = num_dual::jacobian(
+                |y: Vector6<DualSVec64<6>>| {
+                    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);

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)
+    );
 }

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<T: Scalar>(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<T: Scalar>(period: T, mu: f64) -> T {
+    ((period / TAU).powi(2) * mu).cbrt()
+}
+
+/// Mean motion, in radians per second.
+pub fn mean_motion<T: Scalar>(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<T: Scalar>(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<T: Scalar>(position: Vector3<T>, velocity: Vector3<T>, 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<T: Scalar>(
+    position: Vector3<T>,
+    velocity: Vector3<T>,
+) -> Vector3<T> {
+    position.cross(&velocity)
+}
+
+/// Eccentric anomaly from true anomaly, both in radians.
+pub fn eccentric_from_true<T: Scalar>(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<T: Scalar>(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<T: Scalar>(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<T: Scalar>(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<T: Scalar>(from: Vector3<T>, to: Vector3<T>, unit_normal: Vector3<T>) -> 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<T: Scalar>(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<T: Scalar> {
+    /// 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<T: Scalar> Keplerian<T> {
+    /// 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<T>, velocity: Vector3<T>, 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<T>, Vector3<T>) {
+        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<T>) -> Vector3<T> {
+        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<const D: usize>: HasPosition<D> + HasVelocity<D> {
+    /// Starts a [`State`] from Keplerian elements.
+    fn with_keplerian<T: Scalar>(self, elements: Keplerian<T>, mu: f64) -> State<T, D, Self> {
+        let (position, velocity) = elements.to_cartesian(mu);
+        self.with_position(position).with_velocity(velocity)
+    }
+}
+
+impl<const D: usize, L: HasPosition<D> + HasVelocity<D>> TranslationalLayout<D> for L {}
+
+impl<T: Scalar, const D: usize, L: HasPosition<D> + HasVelocity<D>> State<T, D, L> {
+    /// The Keplerian elements of this state, about a body of parameter `mu`.
+    pub fn keplerian(&self, mu: f64) -> Keplerian<T> {
+        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<T> {
+        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<f64>, Vector3<f64>) {
+        (
+            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<f64, 6>) = num_dual::gradient(
+            |y: Vector6<DualSVec64<6>>| {
+                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<f64>| {
+            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);
+        }
+    }
+}

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,

src/propagation.rs

@@ -88,6 +88,14 @@ pub struct Assembly<T: Scalar, const D: usize, L: Layout<D>> {
 }
 
 impl<T: Scalar, const D: usize, L: Layout<D>> Assembly<T, D, L> {
+    /// 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<Box<dyn DynamicsModel<T, D, L>>>, 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::<f64, 7, OrbitMass> {
+        let assembly: Assembly<f64, 7, OrbitMass> = Assembly::new(
-            models: vec![Box::new(ConstantForce {
+            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::<f64, 7, OrbitMass> {
+        let assembly: Assembly<f64, 7, OrbitMass> = Assembly::new(
-            models: vec![
+            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::<f64, 6, Orbit> {
+        let assembly: Assembly<f64, 6, Orbit> = Assembly::new(
-            models: vec![Box::new(ConstantForce {
+            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);
     }