#![allow(clippy::needless_range_loop)]
use core::fmt;
use crate::gauss_newton::{GaussNewton, GaussNewtonReport, ResidualModel};
use crate::srif::SrifError;
const UNEVALUABLE_RESIDUAL: f64 = 1e15;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct TrajectorySample {
pub t: f64,
pub state: [f64; 6],
}
pub trait SampledPropagator {
fn state_at(&self, t: f64) -> Option<[f64; 6]>;
}
pub trait DifferentiablePropagator<const N: usize>: SampledPropagator {
fn state_and_partials(&self, t: f64) -> Option<([f64; 6], [[f64; N]; 6])>;
}
pub trait PropagatorBuilder<const N: usize> {
type Propagator: SampledPropagator;
fn build(&self, parameters: &[f64; N]) -> Option<Self::Propagator>;
}
#[derive(Debug, Clone, Copy)]
pub struct PropagatorConverter {
pub solver: GaussNewton,
pub position_sigma: f64,
pub velocity_sigma: Option<f64>,
}
impl Default for PropagatorConverter {
fn default() -> Self {
Self {
solver: GaussNewton::default(),
position_sigma: 1.0,
velocity_sigma: None,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct TrajectoryResiduals {
pub position_rms: f64,
pub velocity_rms: f64,
pub worst_position: f64,
pub samples: usize,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ConverterFit<const N: usize> {
pub parameters: [f64; N],
pub covariance: [[f64; N]; N],
pub report: GaussNewtonReport,
pub residuals: TrajectoryResiduals,
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub enum ConverterError {
NoSamples,
NonPositiveSigma,
NonPositiveStep {
parameter: usize,
},
Unbuildable,
Unevaluable {
sample: usize,
},
Solver(SrifError),
}
impl fmt::Display for ConverterError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::NoSamples => write!(f, "no reference samples to fit to"),
Self::NonPositiveSigma => write!(f, "a measurement sigma is not strictly positive"),
Self::NonPositiveStep { parameter } => write!(
f,
"the finite-difference step for parameter {parameter} is not strictly positive"
),
Self::Unbuildable => write!(f, "the propagator refused the parameter vector"),
Self::Unevaluable { sample } => {
write!(f, "the propagator has no state at sample {sample}")
}
Self::Solver(e) => write!(f, "the least-squares solve failed: {e}"),
}
}
}
impl core::error::Error for ConverterError {}
impl From<SrifError> for ConverterError {
fn from(e: SrifError) -> Self {
Self::Solver(e)
}
}
impl PropagatorConverter {
pub fn fit_by_finite_difference<const N: usize, B: PropagatorBuilder<N>>(
&self,
builder: &B,
samples: &[TrajectorySample],
initial: &[f64; N],
steps: &[f64; N],
) -> Result<ConverterFit<N>, ConverterError> {
let weights = self.weights(samples)?;
for (parameter, &step) in steps.iter().enumerate() {
if step <= 0.0 || !step.is_finite() {
return Err(ConverterError::NonPositiveStep { parameter });
}
}
if builder.build(initial).is_none() {
return Err(ConverterError::Unbuildable);
}
let model = FiniteDifferenceModel {
builder,
samples,
steps,
weights,
};
self.finish(&model, builder, samples, initial)
}
pub fn fit_by_jacobian<const N: usize, B: PropagatorBuilder<N>>(
&self,
builder: &B,
samples: &[TrajectorySample],
initial: &[f64; N],
) -> Result<ConverterFit<N>, ConverterError>
where
B::Propagator: DifferentiablePropagator<N>,
{
let weights = self.weights(samples)?;
if builder.build(initial).is_none() {
return Err(ConverterError::Unbuildable);
}
let model = JacobianModel {
builder,
samples,
weights,
};
self.finish(&model, builder, samples, initial)
}
fn weights(&self, samples: &[TrajectorySample]) -> Result<Weights, ConverterError> {
if samples.is_empty() {
return Err(ConverterError::NoSamples);
}
if self.position_sigma <= 0.0 || !self.position_sigma.is_finite() {
return Err(ConverterError::NonPositiveSigma);
}
match self.velocity_sigma {
None => Ok(Weights {
position: 1.0 / (self.position_sigma * self.position_sigma),
velocity: 0.0,
components: 3,
}),
Some(sigma) if sigma > 0.0 && sigma.is_finite() => Ok(Weights {
position: 1.0 / (self.position_sigma * self.position_sigma),
velocity: 1.0 / (sigma * sigma),
components: 6,
}),
Some(_) => Err(ConverterError::NonPositiveSigma),
}
}
fn finish<const N: usize, B: PropagatorBuilder<N>, M: ResidualModel<N>>(
&self,
model: &M,
builder: &B,
samples: &[TrajectorySample],
initial: &[f64; N],
) -> Result<ConverterFit<N>, ConverterError> {
let (parameters, covariance, report) = self.solver.solve(model, initial)?;
let propagator = builder
.build(¶meters)
.ok_or(ConverterError::Unbuildable)?;
let residuals = residual_statistics(&propagator, samples)?;
Ok(ConverterFit {
parameters,
covariance,
report,
residuals,
})
}
}
pub fn residual_statistics<P: SampledPropagator>(
propagator: &P,
samples: &[TrajectorySample],
) -> Result<TrajectoryResiduals, ConverterError> {
if samples.is_empty() {
return Err(ConverterError::NoSamples);
}
let mut position_sum = 0.0;
let mut velocity_sum = 0.0;
let mut worst_position = 0.0_f64;
for (sample, reference) in samples.iter().enumerate() {
let state = propagator
.state_at(reference.t)
.ok_or(ConverterError::Unevaluable { sample })?;
let mut position = 0.0;
let mut velocity = 0.0;
for k in 0..3 {
let dr = state[k] - reference.state[k];
let dv = state[k + 3] - reference.state[k + 3];
position += dr * dr;
velocity += dv * dv;
}
position_sum += position;
velocity_sum += velocity;
worst_position = worst_position.max(libm::sqrt(position));
}
#[allow(clippy::cast_precision_loss)] let count = samples.len() as f64;
Ok(TrajectoryResiduals {
position_rms: libm::sqrt(position_sum / count),
velocity_rms: libm::sqrt(velocity_sum / count),
worst_position,
samples: samples.len(),
})
}
#[derive(Debug, Clone, Copy)]
struct Weights {
position: f64,
velocity: f64,
components: usize,
}
impl Weights {
const fn of(self, component: usize) -> f64 {
if component < 3 {
self.position
} else {
self.velocity
}
}
}
fn emit_sample<const N: usize>(
state: &[f64; 6],
reference: &TrajectorySample,
partials: &[[f64; N]; 6],
weights: Weights,
emit: &mut dyn FnMut(&[f64; N], f64, f64),
) {
for k in 0..weights.components {
emit(&partials[k], state[k] - reference.state[k], weights.of(k));
}
}
fn emit_unevaluable<const N: usize>(weights: Weights, emit: &mut dyn FnMut(&[f64; N], f64, f64)) {
let zero = [0.0; N];
for k in 0..weights.components {
emit(&zero, UNEVALUABLE_RESIDUAL, weights.of(k));
}
}
struct FiniteDifferenceModel<'a, const N: usize, B: PropagatorBuilder<N>> {
builder: &'a B,
samples: &'a [TrajectorySample],
steps: &'a [f64; N],
weights: Weights,
}
impl<const N: usize, B: PropagatorBuilder<N>> ResidualModel<N> for FiniteDifferenceModel<'_, N, B> {
fn residuals(&self, x: &[f64; N], emit: &mut dyn FnMut(&[f64; N], f64, f64)) {
let Some(nominal) = self.builder.build(x) else {
for _ in self.samples {
emit_unevaluable(self.weights, emit);
}
return;
};
let perturbed: [Option<(B::Propagator, B::Propagator)>; N] = core::array::from_fn(|j| {
let mut forward = *x;
forward[j] += self.steps[j];
let mut backward = *x;
backward[j] -= self.steps[j];
Some((
self.builder.build(&forward)?,
self.builder.build(&backward)?,
))
});
for reference in self.samples {
let Some(state) = nominal.state_at(reference.t) else {
emit_unevaluable(self.weights, emit);
continue;
};
let mut partials = [[0.0; N]; 6];
let mut complete = true;
for j in 0..N {
let Some((forward, backward)) = &perturbed[j] else {
complete = false;
break;
};
let (Some(plus), Some(minus)) = (
forward.state_at(reference.t),
backward.state_at(reference.t),
) else {
complete = false;
break;
};
let scale = 0.5 / self.steps[j];
for k in 0..6 {
partials[k][j] = (plus[k] - minus[k]) * scale;
}
}
if complete {
emit_sample(&state, reference, &partials, self.weights, emit);
} else {
emit_unevaluable(self.weights, emit);
}
}
}
}
struct JacobianModel<'a, const N: usize, B: PropagatorBuilder<N>> {
builder: &'a B,
samples: &'a [TrajectorySample],
weights: Weights,
}
impl<const N: usize, B: PropagatorBuilder<N>> ResidualModel<N> for JacobianModel<'_, N, B>
where
B::Propagator: DifferentiablePropagator<N>,
{
fn residuals(&self, x: &[f64; N], emit: &mut dyn FnMut(&[f64; N], f64, f64)) {
let Some(propagator) = self.builder.build(x) else {
for _ in self.samples {
emit_unevaluable(self.weights, emit);
}
return;
};
for reference in self.samples {
match propagator.state_and_partials(reference.t) {
Some((state, partials)) => {
emit_sample(&state, reference, &partials, self.weights, emit);
}
None => emit_unevaluable(self.weights, emit),
}
}
}
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::float_cmp)]
use super::*;
struct Line {
horizon: f64,
}
struct LineArc {
parameters: [f64; 6],
horizon: f64,
}
const DRIFT: [f64; 6] = [1.0, 2.0, 3.0, 0.0, 0.0, 0.0];
impl SampledPropagator for LineArc {
fn state_at(&self, t: f64) -> Option<[f64; 6]> {
if t > self.horizon {
return None;
}
Some(core::array::from_fn(|k| self.parameters[k] + t * DRIFT[k]))
}
}
impl DifferentiablePropagator<6> for LineArc {
fn state_and_partials(&self, t: f64) -> Option<([f64; 6], [[f64; 6]; 6])> {
let state = self.state_at(t)?;
let mut partials = [[0.0; 6]; 6];
for k in 0..6 {
partials[k][k] = 1.0;
}
Some((state, partials))
}
}
impl PropagatorBuilder<6> for Line {
type Propagator = LineArc;
fn build(&self, parameters: &[f64; 6]) -> Option<LineArc> {
if parameters[0] <= 0.0 {
return None;
}
Some(LineArc {
parameters: *parameters,
horizon: self.horizon,
})
}
}
const TRUTH: [f64; 6] = [10.0, 20.0, 30.0, 0.5, -0.5, 0.25];
fn samples(horizon: f64) -> Vec<TrajectorySample> {
(0..20)
.map(|i| {
let t = f64::from(i) * horizon / 19.0;
TrajectorySample {
t,
state: core::array::from_fn(|k| TRUTH[k] + t * DRIFT[k]),
}
})
.collect()
}
#[test]
fn both_jacobian_sources_recover_the_same_parameters() {
let line = Line { horizon: 100.0 };
let samples = samples(100.0);
let converter = PropagatorConverter {
velocity_sigma: Some(1.0),
..PropagatorConverter::default()
};
let guess = [9.0, 19.0, 29.0, 0.0, 0.0, 0.0];
let steps = [1e-4; 6];
let difference = converter
.fit_by_finite_difference(&line, &samples, &guess, &steps)
.unwrap();
let analytic = converter.fit_by_jacobian(&line, &samples, &guess).unwrap();
for k in 0..6 {
assert!(
(difference.parameters[k] - TRUTH[k]).abs() < 1e-9,
"finite difference recovered {} for {}",
difference.parameters[k],
TRUTH[k]
);
assert!(
(analytic.parameters[k] - TRUTH[k]).abs() < 1e-9,
"analytic recovered {} for {}",
analytic.parameters[k],
TRUTH[k]
);
assert!(
(difference.parameters[k] - analytic.parameters[k]).abs() < 1e-10,
"the two Jacobian sources disagree at parameter {k}: {} against {}",
difference.parameters[k],
analytic.parameters[k]
);
}
assert!(difference.residuals.position_rms < 1e-9);
assert!(analytic.residuals.position_rms < 1e-9);
assert_eq!(analytic.residuals.samples, 20);
}
#[test]
fn a_guess_outside_the_domain_is_refused_rather_than_clamped() {
let line = Line { horizon: 100.0 };
let samples = samples(100.0);
assert_eq!(
PropagatorConverter::default()
.fit_by_jacobian(&line, &samples, &[-1.0, 0.0, 0.0, 0.0, 0.0, 0.0])
.unwrap_err(),
ConverterError::Unbuildable
);
}
#[test]
fn a_sample_past_the_propagator_horizon_is_reported() {
let line = Line { horizon: 50.0 };
let samples = samples(100.0);
assert!(matches!(
PropagatorConverter {
velocity_sigma: Some(1.0),
..PropagatorConverter::default()
}
.fit_by_jacobian(&line, &samples, &TRUTH)
.unwrap_err(),
ConverterError::Unevaluable { .. }
));
}
#[test]
fn empty_samples_and_bad_weights_are_refused_before_any_propagation() {
let line = Line { horizon: 100.0 };
assert_eq!(
PropagatorConverter::default()
.fit_by_jacobian(&line, &[], &TRUTH)
.unwrap_err(),
ConverterError::NoSamples
);
let samples = samples(100.0);
assert_eq!(
PropagatorConverter {
position_sigma: 0.0,
..PropagatorConverter::default()
}
.fit_by_jacobian(&line, &samples, &TRUTH)
.unwrap_err(),
ConverterError::NonPositiveSigma
);
assert_eq!(
PropagatorConverter {
velocity_sigma: Some(-1.0),
..PropagatorConverter::default()
}
.fit_by_jacobian(&line, &samples, &TRUTH)
.unwrap_err(),
ConverterError::NonPositiveSigma
);
assert_eq!(
PropagatorConverter {
velocity_sigma: Some(0.0),
..PropagatorConverter::default()
}
.fit_by_jacobian(&line, &samples, &TRUTH)
.unwrap_err(),
ConverterError::NonPositiveSigma
);
assert_eq!(
PropagatorConverter::default()
.fit_by_finite_difference(&line, &samples, &TRUTH, &[1.0, 0.0, 1.0, 1.0, 1.0, 1.0])
.unwrap_err(),
ConverterError::NonPositiveStep { parameter: 1 }
);
}
#[test]
fn an_unobserved_parameter_is_reported_rather_than_invented() {
let line = Line { horizon: 100.0 };
let samples = samples(100.0);
assert!(matches!(
PropagatorConverter::default()
.fit_by_jacobian(&line, &samples, &[9.0, 19.0, 29.0, 0.0, 0.0, 0.0])
.unwrap_err(),
ConverterError::Solver(SrifError::RankDeficient { .. })
));
}
#[test]
fn a_weighted_row_is_the_residual_and_the_weight_the_sigmas_imply() {
let line = Line { horizon: 100.0 };
let converter = PropagatorConverter {
position_sigma: 4.0,
velocity_sigma: Some(0.5),
..PropagatorConverter::default()
};
let samples = [
TrajectorySample {
t: 0.0,
state: [9.0, 22.0, 30.5, 0.25, -0.5, 0.75],
},
TrajectorySample {
t: 2.0,
state: [12.5, 20.0, 36.0, 1.0, -1.5, 0.25],
},
];
let weights = converter.weights(&samples).unwrap();
let model = JacobianModel {
builder: &line,
samples: &samples,
weights,
};
let mut rows = Vec::new();
model.residuals(&TRUTH, &mut |_h, r, w| rows.push((r, w)));
let expected = [
(1.0, 0.0625),
(-2.0, 0.0625),
(-0.5, 0.0625),
(0.25, 4.0),
(0.0, 4.0),
(-0.5, 4.0),
(-0.5, 0.0625),
(4.0, 0.0625),
(0.0, 0.0625),
(-0.5, 4.0),
(1.0, 4.0),
(0.0, 4.0),
];
assert_eq!(rows.len(), expected.len(), "one row per fitted component");
for (k, (&(residual, weight), &(want_residual, want_weight))) in
rows.iter().zip(&expected).enumerate()
{
assert_eq!(residual, want_residual, "the residual of row {k}");
assert_eq!(weight, want_weight, "the weight of row {k}");
}
}
#[test]
fn an_unreachable_sample_emits_a_rejecting_row_rather_than_no_row() {
let line = Line { horizon: 1.0 };
let converter = PropagatorConverter {
position_sigma: 4.0,
velocity_sigma: Some(0.5),
..PropagatorConverter::default()
};
let samples = [
TrajectorySample {
t: 0.0,
state: TRUTH,
},
TrajectorySample {
t: 5.0,
state: TRUTH,
},
];
let weights = converter.weights(&samples).unwrap();
let model = JacobianModel {
builder: &line,
samples: &samples,
weights,
};
let mut rows = Vec::new();
model.residuals(&TRUTH, &mut |h: &[f64; 6], r, w| rows.push((*h, r, w)));
assert_eq!(rows.len(), 12, "both samples emit their six components");
for (k, &(jacobian, residual, _)) in rows.iter().take(6).enumerate() {
assert_eq!(jacobian[k], 1.0, "the unit partial of component {k}");
assert_eq!(residual, 0.0, "the residual at t = 0, where TRUTH is exact");
}
for (k, &(jacobian, residual, weight)) in rows.iter().skip(6).enumerate() {
assert_eq!(
jacobian, [0.0; 6],
"an unevaluable row must leave the information matrix untouched"
);
assert_eq!(residual, UNEVALUABLE_RESIDUAL, "the substitute residual");
assert_eq!(
weight,
if k < 3 { 0.0625 } else { 4.0 },
"an unevaluable row carries the component's own weight"
);
}
}
struct Frozen([f64; 6]);
impl SampledPropagator for Frozen {
fn state_at(&self, _t: f64) -> Option<[f64; 6]> {
Some(self.0)
}
}
#[test]
fn the_reported_residuals_are_the_root_mean_square_of_the_differences() {
let state = [10.0, 20.0, 30.0, 5.0, 7.0, 11.0];
let differences = [
[3.0, 4.0, 12.0, 1.0, 2.0, 2.0],
[2.0, 6.0, 9.0, 2.0, 3.0, 6.0],
[1.0, 3.0, 0.0, 3.0, 4.0, 5.0],
];
let times = [0.0, 10.0, 20.0];
let samples: Vec<TrajectorySample> = times
.iter()
.zip(&differences)
.map(|(&t, d)| TrajectorySample {
t,
state: core::array::from_fn(|k| state[k] - d[k]),
})
.collect();
let residuals = residual_statistics(&Frozen(state), &samples).unwrap();
assert_eq!(residuals.position_rms, 10.0);
assert_eq!(residuals.velocity_rms, 6.0);
assert_eq!(residuals.worst_position, 13.0);
assert_eq!(residuals.samples, 3);
}
#[test]
fn every_converter_error_says_which_one_it_is() {
let messages = [
(ConverterError::NoSamples, "no reference samples to fit to"),
(
ConverterError::NonPositiveSigma,
"a measurement sigma is not strictly positive",
),
(
ConverterError::NonPositiveStep { parameter: 4 },
"the finite-difference step for parameter 4 is not strictly positive",
),
(
ConverterError::Unbuildable,
"the propagator refused the parameter vector",
),
(
ConverterError::Unevaluable { sample: 7 },
"the propagator has no state at sample 7",
),
(
ConverterError::Solver(SrifError::RankDeficient { direction: 2 }),
"the least-squares solve failed: state direction 2 is unobserved (singular information matrix)",
),
];
for (error, expected) in messages {
assert_eq!(format!("{error}"), expected);
let dynamic: &dyn core::error::Error = &error;
assert_eq!(format!("{dynamic}"), expected);
}
}
}