use crate::light_time::{SPEED_OF_LIGHT, Transit, down_leg};
use crate::measurement::Station;
use crate::modifier::{Observable, Partials, partials};
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum Signal {
Instantaneous,
DownLeg {
tolerance: f64,
max_iterations: usize,
},
}
impl Signal {
#[must_use]
pub const fn down_leg() -> Self {
Self::DownLeg {
tolerance: 1e-15,
max_iterations: 20,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Observed {
state: [f64; 6],
transit: Transit,
line_of_sight: [f64; 3],
range_rate: f64,
corrected: bool,
}
impl Observed {
#[must_use]
pub const fn state(&self) -> &[f64; 6] {
&self.state
}
#[must_use]
pub const fn transit(&self) -> Transit {
self.transit
}
#[must_use]
pub const fn line_of_sight(&self) -> [f64; 3] {
self.line_of_sight
}
#[must_use]
pub const fn range_rate(&self) -> f64 {
self.range_rate
}
}
pub fn reduce<E>(signal: Signal, receive: f64, station: &Station, ephemeris: E) -> Option<Observed>
where
E: Fn(f64) -> Option<[f64; 6]>,
{
let (state, transit, corrected) = match signal {
Signal::Instantaneous => {
let state = ephemeris(receive)?;
let transit = Transit {
delay: 0.0,
emit: receive,
receive,
};
(state, transit, false)
}
Signal::DownLeg {
tolerance,
max_iterations,
} => {
let position = |t: f64| ephemeris(t).map(|s| [s[0], s[1], s[2]]);
let transit = down_leg(
receive,
station.position,
position,
tolerance,
max_iterations,
)?;
(ephemeris(transit.emit)?, transit, true)
}
};
let dr = [
state[0] - station.position[0],
state[1] - station.position[1],
state[2] - station.position[2],
];
let range = libm::sqrt(dr[0] * dr[0] + dr[1] * dr[1] + dr[2] * dr[2]);
if !range.is_finite() || range <= 0.0 {
return None;
}
let line_of_sight = [dr[0] / range, dr[1] / range, dr[2] / range];
let range_rate =
line_of_sight[0] * state[3] + line_of_sight[1] * state[4] + line_of_sight[2] * state[5];
Some(Observed {
state,
transit,
line_of_sight,
range_rate,
corrected,
})
}
#[must_use]
pub fn partials_at_emission<const K: usize, O: Observable<K>>(
observable: &O,
observed: &Observed,
acceleration: [f64; 3],
parameters: &[f64; K],
) -> Partials<K> {
let mut computed = partials(observable, observed.state(), parameters);
if !observed.corrected {
return computed;
}
let state_rate = [
observed.state[3],
observed.state[4],
observed.state[5],
acceleration[0],
acceleration[1],
acceleration[2],
];
let mut numerator = 0.0;
for (w, x) in computed.state.iter().zip(&state_rate) {
numerator += w * x;
}
let scale = numerator / (SPEED_OF_LIGHT + observed.range_rate);
for (slot, u) in computed.state.iter_mut().zip(&observed.line_of_sight) {
*slot -= scale * u;
}
computed
}
#[cfg(test)]
mod tests {
#![allow(clippy::expect_used, clippy::float_cmp, clippy::unwrap_used)]
use spacelab_testkit::assert_within_ulps;
use super::*;
use crate::modifier::{Declination, Range, RangeRate, RightAscension};
fn straight(r0: [f64; 3], v: [f64; 3]) -> impl Fn(f64) -> Option<[f64; 6]> {
move |t: f64| {
Some([
r0[0] + v[0] * t,
r0[1] + v[1] * t,
r0[2] + v[2] * t,
v[0],
v[1],
v[2],
])
}
}
fn straight_stm(t: f64) -> [[f64; 6]; 6] {
let mut phi = [[0.0; 6]; 6];
for (i, row) in phi.iter_mut().enumerate() {
row[i] = 1.0;
if i < 3 {
row[i + 3] = t;
}
}
phi
}
const STATION: Station = Station::fixed([1_500_000.0, 0.0, 0.0]);
const OFF_AXIS: Station = Station {
position: [1_500_000.0, -900_000.0, 2_100_000.0],
velocity: [40.0, 110.0, -7.0],
};
const RECEIVE: f64 = 100.0;
fn radial() -> ([f64; 3], [f64; 3]) {
([7_000_000.0, 0.0, 0.0], [7660.0, 0.0, 0.0])
}
fn skewed() -> ([f64; 3], [f64; 3]) {
(
[7_000_000.0, 1_000_000.0, 2_000_000.0],
[1200.0, -6900.0, 3100.0],
)
}
#[test]
fn the_flight_time_is_the_closed_form_of_the_straight_line() {
let (r0, v) = radial();
let observed = reduce(Signal::down_leg(), RECEIVE, &STATION, straight(r0, v))
.expect("a straight line reaches every epoch");
let expected = (r0[0] - STATION.position[0] + v[0] * RECEIVE) / (SPEED_OF_LIGHT + v[0]);
assert_within_ulps(observed.transit().delay, expected, 2, "delay");
assert_within_ulps(observed.transit().emit, RECEIVE - expected, 2, "emit");
assert_within_ulps(observed.range_rate(), v[0], 0, "range rate");
}
#[test]
fn the_path_length_and_the_range_at_emission_are_the_same_number() {
let (r0, v) = radial();
let observed = reduce(Signal::down_leg(), RECEIVE, &STATION, straight(r0, v)).unwrap();
let range = partials_at_emission(&Range { station: STATION }, &observed, [0.0; 3], &[]);
assert_within_ulps(
range.value,
observed.transit().path(),
1,
"path against range",
);
}
#[test]
fn the_instantaneous_convention_changes_nothing_at_all() {
let (r0, v) = radial();
let observed = reduce(Signal::Instantaneous, RECEIVE, &STATION, straight(r0, v)).unwrap();
assert_eq!(observed.transit().delay, 0.0);
assert_eq!(observed.transit().emit, RECEIVE);
let at = straight(r0, v)(RECEIVE).unwrap();
assert_eq!(observed.state(), &at);
let corrected = partials_at_emission(&Range { station: STATION }, &observed, [0.0; 3], &[]);
let plain = crate::modifier::partials(&Range { station: STATION }, &at, &[]);
assert_eq!(corrected.state, plain.state, "the row was corrected anyway");
assert_eq!(corrected.value, plain.value);
}
#[test]
fn a_range_partial_is_the_instantaneous_one_over_one_plus_the_radial_speed_over_c() {
let (r0, v) = radial();
let observed = reduce(Signal::down_leg(), RECEIVE, &STATION, straight(r0, v)).unwrap();
let corrected = partials_at_emission(&Range { station: STATION }, &observed, [0.0; 3], &[]);
let closed = SPEED_OF_LIGHT / (SPEED_OF_LIGHT + v[0]);
assert_within_ulps(corrected.state[0], closed, 2, "d(c tau)/d(x0)");
let relative = (corrected.state[0] - 1.0).abs();
assert!(
(relative - 2.555e-5).abs() < 1e-8,
"the correction is {relative}, not 2.56e-5"
);
}
#[test]
fn the_radial_speed_is_zero_at_the_emission_epoch_not_the_reception_one() {
let (r0, v) = skewed();
let p = OFF_AXIS.position;
let along: f64 = (0..3).map(|k| (p[k] - r0[k]) * v[k]).sum();
let speed_squared: f64 = v.iter().map(|c| c * c).sum();
let closest = along / speed_squared;
let at_closest = straight(r0, v)(closest).unwrap();
let miss: f64 = (0..3)
.map(|k| (at_closest[k] - p[k]).powi(2))
.sum::<f64>()
.sqrt();
let receive = closest + miss / SPEED_OF_LIGHT;
let observed = reduce(Signal::down_leg(), receive, &OFF_AXIS, straight(r0, v)).unwrap();
assert!(
(observed.transit().emit - closest).abs() < 1e-12,
"emission at {}, not at closest approach {closest}",
observed.transit().emit
);
assert!(
observed.range_rate().abs() < 1e-9,
"radial speed {} at closest approach",
observed.range_rate()
);
let corrected =
partials_at_emission(&Range { station: OFF_AXIS }, &observed, [0.0; 3], &[]);
let plain = crate::modifier::partials(&Range { station: OFF_AXIS }, observed.state(), &[]);
for (c, u) in corrected.state.iter().zip(&plain.state) {
assert!(
(c - u).abs() <= 1e-15 * u.abs().max(1.0),
"the correction did not vanish at closest approach: {c} against {u}"
);
}
let late = reduce(Signal::down_leg(), receive, &OFF_AXIS, straight(r0, v)).unwrap();
let at_reception = straight(r0, v)(receive).unwrap();
let d: Vec<f64> = (0..3).map(|k| at_reception[k] - p[k]).collect();
let norm: f64 = d.iter().map(|c| c * c).sum::<f64>().sqrt();
let radial_at_reception: f64 = (0..3).map(|k| d[k] / norm * at_reception[k + 3]).sum();
assert!(
radial_at_reception.abs() > 100.0 * observed.range_rate().abs().max(1e-9),
"the two epochs read the same radial speed, so the test proves nothing: {} against {}",
radial_at_reception,
observed.range_rate()
);
assert!(
(late.range_rate() - observed.range_rate()).abs() < 1e-9,
"the same reception epoch gave two answers"
);
}
#[test]
fn the_rank_one_row_reproduces_a_finite_difference_through_the_whole_solution() {
let (r0, v) = radial();
let r0 = [r0[0], 1_000_000.0, 2_000_000.0];
let v = [v[0], 900.0, -300.0];
let value = |x0: [f64; 6], which: usize| -> f64 {
let eph = straight([x0[0], x0[1], x0[2]], [x0[3], x0[4], x0[5]]);
let o = reduce(Signal::down_leg(), RECEIVE, &STATION, eph).unwrap();
match which {
0 => partials_at_emission(&Range { station: STATION }, &o, [0.0; 3], &[]).value,
1 => {
partials_at_emission(&RightAscension { station: STATION }, &o, [0.0; 3], &[])
.value
}
_ => {
partials_at_emission(&Declination { station: STATION }, &o, [0.0; 3], &[]).value
}
}
};
let x0 = [r0[0], r0[1], r0[2], v[0], v[1], v[2]];
let observed = reduce(Signal::down_leg(), RECEIVE, &STATION, straight(r0, v)).unwrap();
let phi = straight_stm(observed.transit().emit);
for which in 0..3 {
let row = match which {
0 => partials_at_emission(&Range { station: STATION }, &observed, [0.0; 3], &[]),
1 => partials_at_emission(
&RightAscension { station: STATION },
&observed,
[0.0; 3],
&[],
),
_ => partials_at_emission(
&Declination { station: STATION },
&observed,
[0.0; 3],
&[],
),
}
.state;
for j in 0..6 {
let mut analytic = 0.0;
for i in 0..6 {
analytic += row[i] * phi[i][j];
}
let h = if j < 3 { 1.0 } else { 1e-3 };
let mut plus = x0;
let mut minus = x0;
plus[j] += h;
minus[j] -= h;
let numeric = (value(plus, which) - value(minus, which)) / (2.0 * h);
let nominal = value(x0, which).abs();
let floor = 8.0 * f64::EPSILON * nominal / (2.0 * h);
let scale = analytic.abs().max(numeric.abs()).max(1e-12);
assert!(
(analytic - numeric).abs() <= (5e-8 * scale).max(floor),
"observable {which}, component {j}: {analytic} against {numeric}, floor {floor}"
);
}
}
}
#[test]
fn the_acceleration_reaches_a_range_rate_and_nothing_else() {
let (r0, v) = radial();
let r0 = [r0[0], 1_000_000.0, 0.0];
let v = [v[0], 900.0, 0.0];
let observed = reduce(Signal::down_leg(), RECEIVE, &STATION, straight(r0, v)).unwrap();
let gravity = [-8.0, -1.1, 0.0];
for zero_and_gravity in [
(
partials_at_emission(&Range { station: STATION }, &observed, [0.0; 3], &[]),
partials_at_emission(&Range { station: STATION }, &observed, gravity, &[]),
),
(
partials_at_emission(&Declination { station: STATION }, &observed, [0.0; 3], &[]),
partials_at_emission(&Declination { station: STATION }, &observed, gravity, &[]),
),
] {
assert_eq!(
zero_and_gravity.0.state, zero_and_gravity.1.state,
"the acceleration moved a partial whose velocity rows are zero"
);
}
let still = partials_at_emission(&RangeRate { station: STATION }, &observed, [0.0; 3], &[]);
let falling =
partials_at_emission(&RangeRate { station: STATION }, &observed, gravity, &[]);
assert!(
still.state != falling.state,
"a range-rate partial ignored the acceleration"
);
}
#[test]
fn the_line_of_sight_and_the_radial_speed_are_the_emission_geometry() {
let (r0, v) = skewed();
let observed = reduce(Signal::down_leg(), RECEIVE, &OFF_AXIS, straight(r0, v)).unwrap();
let at_emission = straight(r0, v)(observed.transit().emit).unwrap();
let d: Vec<f64> = (0..3)
.map(|k| at_emission[k] - OFF_AXIS.position[k])
.collect();
let norm: f64 = d.iter().map(|c| c * c).sum::<f64>().sqrt();
for (k, u) in observed.line_of_sight().iter().enumerate() {
assert!(u.abs() > 0.01, "component {k} of the line of sight is {u}");
assert!(
(u - d[k] / norm).abs() < 1e-15,
"line of sight component {k}: {u} against {}",
d[k] / norm
);
}
let radial: f64 = (0..3).map(|k| d[k] / norm * at_emission[k + 3]).sum();
assert!(
radial.abs() > 100.0,
"the geometry is nearly transverse: {radial} m/s"
);
assert!(
(observed.range_rate() - radial).abs() < 1e-9,
"radial speed {} against {radial}",
observed.range_rate()
);
let relative: f64 = (0..3)
.map(|k| d[k] / norm * (at_emission[k + 3] - OFF_AXIS.velocity[k]))
.sum();
assert!(
(observed.range_rate() - relative).abs() > 1.0,
"the two range rates are indistinguishable here, so the test proves nothing"
);
}
#[test]
fn a_gap_and_a_zero_baseline_are_refused_rather_than_returned() {
let gap = |_: f64| None;
assert!(reduce(Signal::down_leg(), RECEIVE, &STATION, gap).is_none());
assert!(reduce(Signal::Instantaneous, RECEIVE, &STATION, gap).is_none());
let p = STATION.position;
let at_the_station = move |_: f64| Some([p[0], p[1], p[2], 0.0, 0.0, 0.0]);
assert!(reduce(Signal::Instantaneous, RECEIVE, &STATION, at_the_station).is_none());
let (r0, v) = radial();
let starved = Signal::DownLeg {
tolerance: 1e-15,
max_iterations: 1,
};
assert!(reduce(starved, RECEIVE, &STATION, straight(r0, v)).is_none());
}
}