use spacelab_autodiff::{Dual, Scalar, jacobian};
use crate::light_time::SPEED_OF_LIGHT;
use crate::measurement::{Station, range, range_rate};
use crate::modifier::Observable;
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum MultistaticError {
NonCausalLeg {
emitted: f64,
received: f64,
},
NonPositiveFrequency {
frequency: f64,
},
}
impl core::fmt::Display for MultistaticError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::NonCausalLeg { emitted, received } => write!(
f,
"a leg emitted at {emitted} cannot be received at {received}"
),
Self::NonPositiveFrequency { frequency } => {
write!(f, "carrier frequency {frequency} Hz is not positive")
}
}
}
}
impl core::error::Error for MultistaticError {}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Terminal {
pub station: Station,
pub epoch: f64,
}
impl Terminal {
#[must_use]
pub const fn new(station: Station, epoch: f64) -> Self {
Self { station, epoch }
}
#[must_use]
pub const fn fixed(position: [f64; 3], epoch: f64) -> Self {
Self {
station: Station::fixed(position),
epoch,
}
}
}
fn leg_time(satellite: &[f64; 3], terminal: &Terminal) -> f64 {
let p = terminal.station.position;
let d = [
satellite[0] - p[0],
satellite[1] - p[1],
satellite[2] - p[2],
];
libm::sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]) / SPEED_OF_LIGHT
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct BistaticGeometry {
transmitter: Terminal,
bounce: f64,
receiver: Terminal,
}
impl BistaticGeometry {
pub fn new(
transmitter: Terminal,
bounce: f64,
receiver: Terminal,
) -> Result<Self, MultistaticError> {
causal(transmitter.epoch, bounce)?;
causal(bounce, receiver.epoch)?;
Ok(Self {
transmitter,
bounce,
receiver,
})
}
#[must_use]
pub const fn transmitter(&self) -> Terminal {
self.transmitter
}
#[must_use]
pub const fn bounce(&self) -> f64 {
self.bounce
}
#[must_use]
pub const fn receiver(&self) -> Terminal {
self.receiver
}
#[must_use]
pub fn leg_residuals(&self, satellite: &[f64; 3]) -> [f64; 2] {
[
(self.bounce - self.transmitter.epoch) - leg_time(satellite, &self.transmitter),
(self.receiver.epoch - self.bounce) - leg_time(satellite, &self.receiver),
]
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct TdoaGeometry {
emission: f64,
first: Terminal,
second: Terminal,
}
impl TdoaGeometry {
pub fn new(emission: f64, first: Terminal, second: Terminal) -> Result<Self, MultistaticError> {
causal(emission, first.epoch)?;
causal(emission, second.epoch)?;
Ok(Self {
emission,
first,
second,
})
}
#[must_use]
pub const fn emission(&self) -> f64 {
self.emission
}
#[must_use]
pub const fn first(&self) -> Terminal {
self.first
}
#[must_use]
pub const fn second(&self) -> Terminal {
self.second
}
#[must_use]
pub fn observed(&self) -> f64 {
self.second.epoch - self.first.epoch
}
#[must_use]
pub fn leg_residuals(&self, satellite: &[f64; 3]) -> [f64; 2] {
[
(self.first.epoch - self.emission) - leg_time(satellite, &self.first),
(self.second.epoch - self.emission) - leg_time(satellite, &self.second),
]
}
}
fn causal(emitted: f64, received: f64) -> Result<(), MultistaticError> {
if emitted < received {
Ok(())
} else {
Err(MultistaticError::NonCausalLeg { emitted, received })
}
}
#[must_use]
pub fn bistatic_range<S: Scalar>(satellite: &[S; 6], geometry: &BistaticGeometry) -> S {
range(satellite, &geometry.transmitter.station) + range(satellite, &geometry.receiver.station)
}
#[must_use]
pub fn bistatic_range_rate<S: Scalar>(satellite: &[S; 6], geometry: &BistaticGeometry) -> S {
range_rate(satellite, &geometry.transmitter.station)
+ range_rate(satellite, &geometry.receiver.station)
}
#[must_use]
pub fn tdoa<S: Scalar>(satellite: &[S; 6], geometry: &TdoaGeometry) -> S {
(range(satellite, &geometry.second.station) - range(satellite, &geometry.first.station))
/ S::from_f64(SPEED_OF_LIGHT)
}
#[must_use]
pub fn fdoa<S: Scalar>(satellite: &[S; 6], geometry: &TdoaGeometry, frequency: f64) -> S {
S::from_f64(frequency)
* (range_rate(satellite, &geometry.first.station)
- range_rate(satellite, &geometry.second.station))
/ S::from_f64(SPEED_OF_LIGHT)
}
#[must_use]
pub fn inter_satellite_range<S: Scalar>(local: &[S; 6], remote: &[S; 6]) -> S {
let d = [
local[0] - remote[0],
local[1] - remote[1],
local[2] - remote[2],
];
(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt()
}
#[must_use]
pub fn inter_satellite_range_rate<S: Scalar>(local: &[S; 6], remote: &[S; 6]) -> S {
let d = [
local[0] - remote[0],
local[1] - remote[1],
local[2] - remote[2],
];
let w = [
local[3] - remote[3],
local[4] - remote[4],
local[5] - remote[5],
];
let separation2 = d[0] * d[0] + d[1] * d[1] + d[2] * d[2];
(d[0] * w[0] + d[1] * w[1] + d[2] * w[2]) * separation2.sqrt().recip()
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct BistaticRange {
pub geometry: BistaticGeometry,
}
impl<const K: usize> Observable<K> for BistaticRange {
fn value<S: Scalar>(&self, satellite: &[S; 6], _parameters: &[S; K]) -> S {
bistatic_range(satellite, &self.geometry)
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct BistaticRangeRate {
pub geometry: BistaticGeometry,
}
impl<const K: usize> Observable<K> for BistaticRangeRate {
fn value<S: Scalar>(&self, satellite: &[S; 6], _parameters: &[S; K]) -> S {
bistatic_range_rate(satellite, &self.geometry)
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Tdoa {
pub geometry: TdoaGeometry,
}
impl<const K: usize> Observable<K> for Tdoa {
fn value<S: Scalar>(&self, satellite: &[S; 6], _parameters: &[S; K]) -> S {
tdoa(satellite, &self.geometry)
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Fdoa {
geometry: TdoaGeometry,
frequency: f64,
}
impl Fdoa {
pub fn new(geometry: TdoaGeometry, frequency: f64) -> Result<Self, MultistaticError> {
if frequency > 0.0 {
Ok(Self {
geometry,
frequency,
})
} else {
Err(MultistaticError::NonPositiveFrequency { frequency })
}
}
#[must_use]
pub const fn geometry(&self) -> TdoaGeometry {
self.geometry
}
#[must_use]
pub const fn frequency(&self) -> f64 {
self.frequency
}
}
impl<const K: usize> Observable<K> for Fdoa {
fn value<S: Scalar>(&self, satellite: &[S; 6], _parameters: &[S; K]) -> S {
fdoa(satellite, &self.geometry, self.frequency)
}
}
pub trait PairObservable<const K: usize> {
fn value<S: Scalar>(&self, local: &[S; 6], remote: &[S; 6], parameters: &[S; K]) -> S;
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct InterSatelliteRange;
impl<const K: usize> PairObservable<K> for InterSatelliteRange {
fn value<S: Scalar>(&self, local: &[S; 6], remote: &[S; 6], _parameters: &[S; K]) -> S {
inter_satellite_range(local, remote)
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct InterSatelliteRangeRate;
impl<const K: usize> PairObservable<K> for InterSatelliteRangeRate {
fn value<S: Scalar>(&self, local: &[S; 6], remote: &[S; 6], _parameters: &[S; K]) -> S {
inter_satellite_range_rate(local, remote)
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PairPartials<const K: usize> {
pub value: f64,
pub local: [f64; 6],
pub remote: [f64; 6],
pub parameters: [f64; K],
}
#[must_use]
pub fn pair_partials<const K: usize, O: PairObservable<K>>(
observable: &O,
local: &[f64; 6],
remote: &[f64; 6],
parameters: &[f64; K],
) -> PairPartials<K> {
let mut lifted_parameters = [Dual::<6>::constant(0.0); K];
for (slot, p) in lifted_parameters.iter_mut().zip(parameters) {
*slot = Dual::constant(*p);
}
let frozen_remote = constants::<6>(remote);
let (value, local_jacobian) = jacobian::<6, 1, _>(
|s| [observable.value(s, &frozen_remote, &lifted_parameters)],
local,
);
let frozen_local = constants::<6>(local);
let (_, remote_jacobian) = jacobian::<6, 1, _>(
|s| [observable.value(&frozen_local, s, &lifted_parameters)],
remote,
);
let mut lifted_local = [Dual::<K>::constant(0.0); 6];
let mut lifted_remote = [Dual::<K>::constant(0.0); 6];
for i in 0..6 {
lifted_local[i] = Dual::constant(local[i]);
lifted_remote[i] = Dual::constant(remote[i]);
}
let (_, parameter_jacobian) = jacobian::<K, 1, _>(
|p| [observable.value(&lifted_local, &lifted_remote, p)],
parameters,
);
PairPartials {
value: value[0],
local: first_column(&local_jacobian),
remote: first_column(&remote_jacobian),
parameters: first_column(¶meter_jacobian),
}
}
fn constants<const N: usize>(state: &[f64; 6]) -> [Dual<N>; 6] {
let mut lifted = [Dual::<N>::constant(0.0); 6];
for (slot, x) in lifted.iter_mut().zip(state) {
*slot = Dual::constant(*x);
}
lifted
}
fn first_column<const N: usize>(jacobian: &[[f64; 1]; N]) -> [f64; N] {
let mut column = [0.0; N];
for (slot, row) in column.iter_mut().zip(jacobian) {
*slot = row[0];
}
column
}
#[cfg(test)]
mod tests {
#![allow(
clippy::excessive_precision,
clippy::unreadable_literal,
clippy::expect_used,
clippy::unwrap_used,
clippy::needless_range_loop
)]
use spacelab_testkit::assert_within_ulps;
use super::*;
const SATELLITE: [f64; 6] = [
3_961_000.0,
2_614_000.0,
5_215_000.0,
-3_140.0,
6_190.0,
-700.0,
];
const REMOTE: [f64; 6] = [
4_512_000.0,
1_878_000.0,
4_903_000.0,
-2_050.0,
5_780.0,
2_400.0,
];
const CARRIER: f64 = 2.2e9;
fn first_terminal() -> Terminal {
Terminal::new(
Station {
position: [4_075_530.0, 931_781.0, 4_801_618.0],
velocity: [-67.9, 297.2, 0.0],
},
0.0,
)
}
fn second_terminal() -> Terminal {
Terminal::new(
Station {
position: [3_183_899.0, 1_421_478.0, 5_322_820.0],
velocity: [-103.6, 232.2, 0.0],
},
1.0,
)
}
fn bistatic() -> BistaticGeometry {
let mut receiver = second_terminal();
receiver.epoch = 2.0;
BistaticGeometry::new(first_terminal(), 1.0, receiver).expect("an ordered geometry")
}
fn tdoa_pair() -> TdoaGeometry {
let mut second = second_terminal();
second.epoch = 2.0;
TdoaGeometry::new(
0.0,
{
let mut first = first_terminal();
first.epoch = 1.0;
first
},
second,
)
.expect("an ordered geometry")
}
#[test]
fn the_two_leg_observables_are_the_single_leg_ones_combined() {
let (b, t) = (bistatic(), tdoa_pair());
let tx = b.transmitter().station;
let rx = b.receiver().station;
assert_within_ulps(
bistatic_range(&SATELLITE, &b),
range(&SATELLITE, &tx) + range(&SATELLITE, &rx),
0,
"the bistatic range",
);
assert_within_ulps(
bistatic_range_rate(&SATELLITE, &b),
range_rate(&SATELLITE, &tx) + range_rate(&SATELLITE, &rx),
0,
"the bistatic range rate",
);
let (one, two) = (t.first().station, t.second().station);
assert_within_ulps(
tdoa(&SATELLITE, &t),
(range(&SATELLITE, &two) - range(&SATELLITE, &one)) / SPEED_OF_LIGHT,
0,
"the TDOA",
);
assert_within_ulps(
fdoa(&SATELLITE, &t, CARRIER),
CARRIER * (range_rate(&SATELLITE, &one) - range_rate(&SATELLITE, &two))
/ SPEED_OF_LIGHT,
0,
"the FDOA",
);
}
#[test]
fn every_observable_matches_the_maxima_closed_form() {
let (b, t) = (bistatic(), tdoa_pair());
assert_within_ulps(
bistatic_range(&SATELLITE, &b),
3.16350019133434775975857394752e6,
0,
"the bistatic range against Maxima",
);
assert_within_ulps(
bistatic_range_rate(&SATELLITE, &b),
9.12320439456152896377995923281e3,
0,
"the bistatic range rate against Maxima",
);
assert_within_ulps(
tdoa(&SATELLITE, &t),
-1.02936463346488750872129618964e-3,
1,
"the TDOA against Maxima",
);
assert_within_ulps(
fdoa(&SATELLITE, &t, CARRIER),
1.73842815162547150815909549816e4,
0,
"the FDOA against Maxima",
);
assert_within_ulps(
inter_satellite_range(&SATELLITE, &REMOTE),
9.70897007926175452790431266953e5,
0,
"the inter-satellite range against Maxima",
);
assert_within_ulps(
inter_satellite_range_rate(&SATELLITE, &REMOTE),
-6.67939024124905219627190708466e1,
0,
"the inter-satellite range rate against Maxima",
);
}
#[test]
fn the_bistatic_gradient_is_the_sum_of_the_two_unit_vectors() {
let b = bistatic();
let (_, jac) = jacobian::<6, 1, _>(|s| [bistatic_range(s, &b)], &SATELLITE);
let maxima = [
4.78425527005991124489541937078e-1,
1.80441329126225152612351648908e0,
1.62583515565325185868685719401e-1,
];
for i in 0..3 {
assert_within_ulps(jac[i][0], maxima[i], 1, "a bistatic position partial");
}
for i in 3..6 {
assert_within_ulps(jac[i][0], 0.0, 0, "a bistatic velocity partial");
}
}
#[test]
fn the_tdoa_gradient_is_the_difference_of_the_two_unit_vectors() {
let t = tdoa_pair();
let (_, jac) = jacobian::<6, 1, _>(|s| [tdoa(s, &t)], &SATELLITE);
let unit = |terminal: &Terminal| {
let p = terminal.station.position;
let d = [
SATELLITE[0] - p[0],
SATELLITE[1] - p[1],
SATELLITE[2] - p[2],
];
let n = libm::sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
[d[0] / n, d[1] / n, d[2] / n]
};
let (one, two) = (unit(&t.first()), unit(&t.second()));
let mut norm = 0.0;
for i in 0..3 {
let want = (two[i] - one[i]) / SPEED_OF_LIGHT;
assert!(
(jac[i][0] - want).abs() <= 1e-14 * want.abs(),
"∂TDOA/∂r[{i}] = {} against {want}",
jac[i][0]
);
norm += jac[i][0] * jac[i][0];
}
let norm = libm::sqrt(norm);
assert!(
norm < 2.0 / SPEED_OF_LIGHT,
"a TDOA gradient of {norm} exceeds the 2/c ceiling"
);
}
#[test]
fn the_tdoa_vanishes_on_the_perpendicular_bisector() {
let first = Terminal::fixed([-1.0e6, 0.0, 0.0], 1.0);
let second = Terminal::fixed([1.0e6, 0.0, 0.0], 1.0);
let geometry = TdoaGeometry::new(0.0, first, second).expect("equal epochs are legal");
let overhead = [0.0, 3.0e6, 4.0e6, 1.0, 2.0, 3.0];
assert_within_ulps(
tdoa(&overhead, &geometry),
0.0,
0,
"the TDOA on the bisector",
);
assert_within_ulps(geometry.observed(), 0.0, 0, "the observed TDOA");
}
#[test]
fn the_fdoa_sign_follows_the_closing_terminal() {
let t = tdoa_pair();
let (one, two) = (t.first().station, t.second().station);
let approaching_second = fdoa(&SATELLITE, &t, CARRIER);
assert!(
range_rate(&SATELLITE, &one) > range_rate(&SATELLITE, &two),
"the fixture must have the first leg opening faster",
);
assert!(
approaching_second > 0.0,
"the terminal the emitter recedes from slower must see the higher frequency, got {approaching_second}"
);
}
#[test]
fn a_non_causal_leg_is_refused_at_construction() {
let a = Terminal::fixed([1.0e6, 0.0, 0.0], 0.0);
let b = Terminal::fixed([0.0, 1.0e6, 0.0], 2.0);
assert_eq!(
BistaticGeometry::new(a, -1.0, b),
Err(MultistaticError::NonCausalLeg {
emitted: 0.0,
received: -1.0,
})
);
assert_eq!(
BistaticGeometry::new(a, 3.0, b),
Err(MultistaticError::NonCausalLeg {
emitted: 3.0,
received: 2.0,
})
);
assert!(BistaticGeometry::new(a, 0.0, b).is_err());
assert!(TdoaGeometry::new(2.0, a, b).is_err());
assert!(TdoaGeometry::new(-1.0, a, b).is_ok());
}
#[test]
fn the_leg_residuals_expose_epochs_that_do_not_match_the_geometry() {
let position = [SATELLITE[0], SATELLITE[1], SATELLITE[2]];
let mut transmitter = first_terminal();
let mut receiver = second_terminal();
transmitter.epoch = 0.0;
let up = leg_time(&position, &transmitter);
let down = leg_time(&position, &receiver);
receiver.epoch = up + down;
let solved = BistaticGeometry::new(transmitter, up, receiver).expect("ordered");
for residual in solved.leg_residuals(&position) {
assert!(
libm::fabs(residual) < 1e-18,
"a solved geometry left {residual} s of leg residual"
);
}
let shared = TdoaGeometry::new(
0.0,
{
let mut t = first_terminal();
t.epoch = 1.0;
t
},
{
let mut t = second_terminal();
t.epoch = 1.0;
t
},
)
.expect("ordered");
let residuals = shared.leg_residuals(&position);
assert!(
libm::fabs(residuals[0] - (1.0 - up)) < 1e-15
&& libm::fabs(residuals[1] - (1.0 - down)) < 1e-15,
"the shared-epoch residuals {residuals:?} do not name the missing flight times"
);
assert_within_ulps(shared.observed(), 0.0, 0, "a shared epoch reads zero TDOA");
}
#[test]
fn the_leg_flight_time_is_the_leg_range_over_c() {
let position = [SATELLITE[0], SATELLITE[1], SATELLITE[2]];
assert_within_ulps(
leg_time(&position, &first_terminal()),
5.79083271164055675258452927652e-3,
0,
"the first leg's flight time against Maxima",
);
assert_within_ulps(
leg_time(&position, &second_terminal()),
4.76146807817566924386323308688e-3,
0,
"the second leg's flight time against Maxima",
);
}
#[test]
fn the_leg_residuals_are_the_epoch_gaps_minus_the_flight_times() {
let position = [SATELLITE[0], SATELLITE[1], SATELLITE[2]];
let mut transmitter = first_terminal();
transmitter.epoch = 0.25;
let mut receiver = second_terminal();
receiver.epoch = 1.5;
let unsolved = BistaticGeometry::new(transmitter, 0.75, receiver).expect("ordered");
let residuals = unsolved.leg_residuals(&position);
assert_within_ulps(
residuals[0],
4.94209167288359443247415470723e-1,
0,
"the bistatic up-leg residual against Maxima",
);
assert_within_ulps(
residuals[1],
7.45238531921824330756136766913e-1,
0,
"the bistatic down-leg residual against Maxima",
);
let mut first = first_terminal();
first.epoch = 1.25;
let mut second = second_terminal();
second.epoch = 1.75;
let pair = TdoaGeometry::new(0.5, first, second).expect("ordered");
let residuals = pair.leg_residuals(&position);
assert_within_ulps(
residuals[0],
7.44209167288359443247415470723e-1,
0,
"the TDOA first-leg residual against Maxima",
);
assert_within_ulps(
residuals[1],
1.24523853192182433075613676691e0,
0,
"the TDOA second-leg residual against Maxima",
);
}
#[test]
fn a_geometry_reports_the_epochs_it_was_built_from() {
let mut transmitter = first_terminal();
transmitter.epoch = 0.25;
let mut receiver = second_terminal();
receiver.epoch = 1.5;
let b = BistaticGeometry::new(transmitter, 0.75, receiver).expect("ordered");
assert_within_ulps(b.transmitter().epoch, 0.25, 0, "the transmission epoch");
assert_within_ulps(b.bounce(), 0.75, 0, "the reflection epoch");
assert_within_ulps(b.receiver().epoch, 1.5, 0, "the reception epoch");
let mut first = first_terminal();
first.epoch = 1.25;
let mut second = second_terminal();
second.epoch = 1.75;
let t = TdoaGeometry::new(0.5, first, second).expect("ordered");
assert_within_ulps(t.emission(), 0.5, 0, "the emission epoch");
assert_within_ulps(t.first().epoch, 1.25, 0, "the first reception epoch");
assert_within_ulps(t.second().epoch, 1.75, 0, "the second reception epoch");
assert_within_ulps(t.observed(), 0.5, 0, "the observed TDOA");
}
#[test]
fn every_refusal_names_the_epochs_or_the_frequency_it_refused() {
let messages = [
(
MultistaticError::NonCausalLeg {
emitted: 2.0,
received: 1.5,
},
"a leg emitted at 2 cannot be received at 1.5",
),
(
MultistaticError::NonPositiveFrequency { frequency: -3.25 },
"carrier frequency -3.25 Hz is not positive",
),
];
for (error, expected) in messages {
assert_eq!(format!("{error}"), expected);
let dynamic: &dyn core::error::Error = &error;
assert_eq!(format!("{dynamic}"), expected);
}
}
#[test]
fn a_non_positive_carrier_frequency_is_refused() {
assert_eq!(
Fdoa::new(tdoa_pair(), 0.0),
Err(MultistaticError::NonPositiveFrequency { frequency: 0.0 })
);
assert!(Fdoa::new(tdoa_pair(), -1.0).is_err());
let ok = Fdoa::new(tdoa_pair(), CARRIER).expect("a real carrier");
assert_within_ulps(ok.frequency(), CARRIER, 0, "the carried frequency");
assert_eq!(ok.geometry(), tdoa_pair());
}
#[test]
fn every_observable_reproduces_the_free_function_to_the_ulp() {
let none: [f64; 0] = [];
let (b, t) = (bistatic(), tdoa_pair());
assert_within_ulps(
BistaticRange { geometry: b }.value(&SATELLITE, &none),
bistatic_range(&SATELLITE, &b),
0,
"the bistatic range",
);
assert_within_ulps(
BistaticRangeRate { geometry: b }.value(&SATELLITE, &none),
bistatic_range_rate(&SATELLITE, &b),
0,
"the bistatic range rate",
);
assert_within_ulps(
Tdoa { geometry: t }.value(&SATELLITE, &none),
tdoa(&SATELLITE, &t),
0,
"the TDOA",
);
assert_within_ulps(
Fdoa::new(t, CARRIER)
.expect("a real carrier")
.value(&SATELLITE, &none),
fdoa(&SATELLITE, &t, CARRIER),
0,
"the FDOA",
);
}
#[test]
fn a_link_to_a_known_remote_is_the_moving_station_range() {
let none: [f64; 0] = [];
let station = Station {
position: [REMOTE[0], REMOTE[1], REMOTE[2]],
velocity: [REMOTE[3], REMOTE[4], REMOTE[5]],
};
let p = pair_partials(&InterSatelliteRange, &SATELLITE, &REMOTE, &none);
assert_within_ulps(
p.value,
range(&SATELLITE, &station),
0,
"the link range against the moving-station range",
);
assert_within_ulps(
pair_partials(&InterSatelliteRangeRate, &SATELLITE, &REMOTE, &none).value,
range_rate(&SATELLITE, &station),
0,
"the link rate against the moving-station range rate",
);
let (_, jac) = jacobian::<6, 1, _>(|s| [range(s, &station)], &SATELLITE);
for i in 0..6 {
assert_within_ulps(p.local[i], jac[i][0], 0, "a local partial");
}
}
#[test]
fn the_pair_jacobian_is_antisymmetric_for_a_separation() {
let none: [f64; 0] = [];
let p = pair_partials(&InterSatelliteRange, &SATELLITE, &REMOTE, &none);
for i in 0..6 {
assert_within_ulps(p.remote[i], -p.local[i], 0, "the mirrored partial");
}
let rate = pair_partials(&InterSatelliteRangeRate, &SATELLITE, &REMOTE, &none);
for i in 0..6 {
assert_within_ulps(
rate.remote[i],
-rate.local[i],
0,
"the mirrored rate partial",
);
}
let speed_partial = libm::sqrt((3..6).map(|i| rate.local[i] * rate.local[i]).sum::<f64>());
assert!(
(speed_partial - 1.0).abs() < 1e-15,
"the rate's velocity gradient is a unit line of sight, got {speed_partial}"
);
}
#[test]
fn an_estimated_parameter_reaches_the_pair_partials() {
struct BiasedLink;
impl PairObservable<2> for BiasedLink {
fn value<S: Scalar>(&self, local: &[S; 6], remote: &[S; 6], parameters: &[S; 2]) -> S {
inter_satellite_range(local, remote) + parameters[1]
}
}
let p = pair_partials(&BiasedLink, &SATELLITE, &REMOTE, &[3.0, -7.5]);
let bare = pair_partials(&InterSatelliteRange, &SATELLITE, &REMOTE, &[0.0; 2]);
assert_within_ulps(p.value, bare.value - 7.5, 0, "the biased value");
assert_within_ulps(p.parameters[1], 1.0, 0, "∂link/∂bias");
assert_within_ulps(p.parameters[0], 0.0, 0, "∂link/∂(unused parameter)");
for i in 0..6 {
assert_within_ulps(p.local[i], bare.local[i], 0, "a local partial");
assert_within_ulps(p.remote[i], bare.remote[i], 0, "a remote partial");
}
}
}