use crate::astro::frames::transforms::itrs_to_geodetic_compute;
use std::f64::consts::PI;
use crate::astro::time::civil;
use crate::astro::time::model::{Instant, JulianDateSplit, TimeScale};
use crate::constants::{
AZIMUTH_ZENITH_EPS, C_M_S, DEGREES_PER_CIRCLE, DEGREES_PER_SEMICIRCLE, F_L1_HZ, J2000_JD,
KM_TO_M, MICROSECONDS_PER_SECOND, OBSERVABLE_TRANSMIT_TIME_ITERATIONS, OMEGA_E_DOT_RAD_S,
SECONDS_PER_DAY,
};
use crate::ephemeris::BroadcastEphemeris;
use crate::estimation::recipe::SagnacRecipe;
use crate::frame::Wgs84Geodetic;
use crate::id::GnssSatelliteId;
use crate::ionex::{ionex_slant_delay, ionosphere_delay, Ionex, IonoModel};
use crate::sp3::Sp3;
use crate::spp::EphemerisSource;
use crate::tropo::{tropo_mapping, tropo_zenith, MappingModel, Met, TropoModel};
use crate::validate;
use crate::Error;
use rayon::prelude::*;
const FD_HALF_S: f64 = 0.5;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ObservableState {
pub position_ecef_m: [f64; 3],
pub clock_s: Option<f64>,
}
pub const OBSERVABLE_STATE_MISSING_POSITION_ECEF_M: [f64; 3] = [f64::NAN; 3];
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ObservableStateElementStatus {
Valid,
Gap,
Error,
}
#[derive(Debug, Clone, PartialEq)]
pub struct ObservableStateBatch {
pub positions_ecef_m: Vec<[f64; 3]>,
pub clocks_s: Vec<Option<f64>>,
pub element_results: Vec<Result<(), ObservablesError>>,
}
pub trait ObservableEphemerisSource {
fn observable_state_at_j2000_s(
&self,
sat: GnssSatelliteId,
t_j2000_s: f64,
) -> Result<ObservableState, ObservablesError>;
fn observable_states_at_j2000_s(
&self,
satellites: &[GnssSatelliteId],
epochs_j2000_s: &[f64],
) -> Result<ObservableStateBatch, ObservablesError> {
if satellites.len() != epochs_j2000_s.len() {
return Err(ObservablesError::InvalidInput {
field: "epochs_j2000_s",
kind: ObservablesInputErrorKind::OutOfRange,
});
}
let mut batch = ObservableStateBatch::with_capacity(satellites.len());
for (&sat, &epoch_j2000_s) in satellites.iter().zip(epochs_j2000_s.iter()) {
batch.push_state_result(self.observable_state_at_j2000_s(sat, epoch_j2000_s));
}
Ok(batch)
}
fn observable_states_at_shared_j2000_s(
&self,
satellites: &[GnssSatelliteId],
epoch_j2000_s: f64,
) -> ObservableStateBatch {
let mut batch = ObservableStateBatch::with_capacity(satellites.len());
for &sat in satellites {
batch.push_state_result(self.observable_state_at_j2000_s(sat, epoch_j2000_s));
}
batch
}
}
impl ObservableEphemerisSource for Sp3 {
fn observable_state_at_j2000_s(
&self,
sat: GnssSatelliteId,
t_j2000_s: f64,
) -> Result<ObservableState, ObservablesError> {
let state = self
.position_at_j2000_seconds(sat, t_j2000_s)
.map_err(ObservablesError::Ephemeris)?;
Ok(ObservableState {
position_ecef_m: state.position.as_array(),
clock_s: state.clock_s,
})
}
}
impl ObservableEphemerisSource for BroadcastEphemeris {
fn observable_state_at_j2000_s(
&self,
sat: GnssSatelliteId,
t_j2000_s: f64,
) -> Result<ObservableState, ObservablesError> {
let Some((position_ecef_m, clock_s)) =
EphemerisSource::position_clock_at_j2000_s(self, sat, t_j2000_s)
else {
return Err(ObservablesError::NoEphemeris);
};
Ok(ObservableState {
position_ecef_m,
clock_s: Some(clock_s),
})
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ObservablesInputErrorKind {
NonFinite,
NotPositive,
Negative,
OutOfRange,
Missing,
FloatParse,
IntParse,
InvalidCivilDate,
InvalidCivilTime,
}
impl core::fmt::Display for ObservablesInputErrorKind {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
let label = match self {
Self::NonFinite => "not finite",
Self::NotPositive => "not positive",
Self::Negative => "negative",
Self::OutOfRange => "out of range",
Self::Missing => "missing",
Self::FloatParse => "invalid float",
Self::IntParse => "invalid integer",
Self::InvalidCivilDate => "invalid civil date",
Self::InvalidCivilTime => "invalid civil time",
};
f.write_str(label)
}
}
impl From<&validate::FieldError> for ObservablesInputErrorKind {
fn from(error: &validate::FieldError) -> Self {
match error {
validate::FieldError::Missing { .. } => Self::Missing,
validate::FieldError::NonFinite { .. } => Self::NonFinite,
validate::FieldError::NotPositive { .. } => Self::NotPositive,
validate::FieldError::Negative { .. } => Self::Negative,
validate::FieldError::OutOfRange { .. } => Self::OutOfRange,
validate::FieldError::FloatParse { .. } => Self::FloatParse,
validate::FieldError::IntParse { .. } => Self::IntParse,
validate::FieldError::InvalidCivilDate { .. } => Self::InvalidCivilDate,
validate::FieldError::InvalidCivilTime { .. } => Self::InvalidCivilTime,
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum ObservablesError {
InvalidInput {
field: &'static str,
kind: ObservablesInputErrorKind,
},
NoEphemeris,
Ephemeris(Error),
}
impl core::fmt::Display for ObservablesError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::InvalidInput { field, kind } => {
write!(f, "invalid observable input {field}: {kind}")
}
Self::NoEphemeris => write!(f, "no ephemeris"),
Self::Ephemeris(err) => write!(f, "{err}"),
}
}
}
impl std::error::Error for ObservablesError {}
impl ObservableStateBatch {
pub fn with_capacity(capacity: usize) -> Self {
Self {
positions_ecef_m: Vec::with_capacity(capacity),
clocks_s: Vec::with_capacity(capacity),
element_results: Vec::with_capacity(capacity),
}
}
pub fn len(&self) -> usize {
self.element_results.len()
}
pub fn is_empty(&self) -> bool {
self.element_results.is_empty()
}
pub fn element(&self, index: usize) -> Option<Result<ObservableState, &ObservablesError>> {
match self.element_results.get(index)? {
Ok(()) => Some(Ok(ObservableState {
position_ecef_m: self.positions_ecef_m[index],
clock_s: self.clocks_s[index],
})),
Err(error) => Some(Err(error)),
}
}
pub fn element_status(&self, index: usize) -> Option<ObservableStateElementStatus> {
match self.element_results.get(index)? {
Ok(()) => Some(ObservableStateElementStatus::Valid),
Err(error) if is_observable_state_gap(error) => Some(ObservableStateElementStatus::Gap),
Err(_) => Some(ObservableStateElementStatus::Error),
}
}
fn push_state_result(&mut self, result: Result<ObservableState, ObservablesError>) {
match result {
Ok(state) => {
self.positions_ecef_m.push(state.position_ecef_m);
self.clocks_s.push(state.clock_s);
self.element_results.push(Ok(()));
}
Err(error) => {
self.positions_ecef_m
.push(OBSERVABLE_STATE_MISSING_POSITION_ECEF_M);
self.clocks_s.push(None);
self.element_results.push(Err(error));
}
}
}
}
pub fn is_observable_state_gap(error: &ObservablesError) -> bool {
matches!(
error,
ObservablesError::NoEphemeris
| ObservablesError::Ephemeris(crate::Error::EpochOutOfRange)
| ObservablesError::Ephemeris(crate::Error::UnknownSatellite(_))
)
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PredictOptions {
pub carrier_hz: f64,
pub light_time: bool,
pub sagnac: bool,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct TransmitTimeOptions {
pub light_time: bool,
pub sagnac: bool,
}
impl Default for TransmitTimeOptions {
fn default() -> Self {
Self {
light_time: true,
sagnac: true,
}
}
}
impl Default for PredictOptions {
fn default() -> Self {
Self {
carrier_hz: F_L1_HZ,
light_time: true,
sagnac: true,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ObservableTroposphereCorrection {
pub met: Met,
pub mapping: MappingModel,
}
impl Default for ObservableTroposphereCorrection {
fn default() -> Self {
Self {
met: Met::new_unchecked(1013.25, 288.15, 0.5),
mapping: MappingModel::Niell,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ObservableIonosphereCorrection<'a> {
Broadcast(IonoModel),
Ionex(&'a Ionex),
}
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct ObservableMediaOptions<'a> {
pub troposphere: Option<ObservableTroposphereCorrection>,
pub ionosphere: Option<ObservableIonosphereCorrection<'a>>,
}
impl ObservableMediaOptions<'_> {
fn is_disabled(self) -> bool {
self.troposphere.is_none() && self.ionosphere.is_none()
}
fn needs_instant(self) -> bool {
self.troposphere.is_some()
|| matches!(
self.ionosphere,
Some(ObservableIonosphereCorrection::Broadcast(_))
)
}
fn needs_carrier(self) -> bool {
self.ionosphere.is_some()
}
fn needs_ionex_epoch(self) -> bool {
matches!(
self.ionosphere,
Some(ObservableIonosphereCorrection::Ionex(_))
)
}
}
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct MediaPredictOptions<'a> {
pub prediction: PredictOptions,
pub media: ObservableMediaOptions<'a>,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct AppliedMediaCorrections {
pub troposphere_m: f64,
pub ionosphere_m: f64,
pub total_m: f64,
}
impl Default for AppliedMediaCorrections {
fn default() -> Self {
Self {
troposphere_m: 0.0,
ionosphere_m: 0.0,
total_m: 0.0,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MediaPredictedObservables {
pub prediction: PredictedObservables,
pub range_m: f64,
pub media: AppliedMediaCorrections,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct TransmitTimeSatelliteState {
pub signal_flight_time_s: f64,
pub transmit_offset_us: i64,
pub transmit_time_j2000_s: f64,
pub clock_s: Option<f64>,
pub transmit_position_ecef_m: [f64; 3],
pub position_ecef_m: [f64; 3],
pub velocity_m_s: [f64; 3],
pub geometric_range_m: f64,
pub los_unit: [f64; 3],
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PredictedObservables {
pub geometric_range_m: f64,
pub range_rate_m_s: f64,
pub doppler_hz: f64,
pub sat_clock_s: Option<f64>,
pub elevation_deg: f64,
pub azimuth_deg: f64,
pub transmit_offset_us: i64,
pub transmit_time_j2000_s: f64,
pub los_unit: [f64; 3],
pub sat_pos_ecef_m: [f64; 3],
pub sat_velocity_m_s: [f64; 3],
}
pub fn j2000_seconds_from_split(jd_whole: f64, jd_fraction: f64) -> Result<f64, ObservablesError> {
validate::finite(jd_whole, "jd_whole").map_err(map_input_error)?;
validate::finite(jd_fraction, "jd_fraction").map_err(map_input_error)?;
validate::finite(
civil::j2000_seconds_from_split(jd_whole, jd_fraction),
"j2000_seconds",
)
.map_err(map_input_error)
}
pub fn observable_media_corrections(
receiver: Wgs84Geodetic,
elevation_rad: f64,
azimuth_rad: f64,
t_rx_j2000_s: f64,
carrier_hz: f64,
options: ObservableMediaOptions<'_>,
) -> Result<AppliedMediaCorrections, ObservablesError> {
if options.is_disabled() {
return Ok(AppliedMediaCorrections::default());
}
validate::finite(elevation_rad, "elevation_rad").map_err(map_input_error)?;
validate::finite(azimuth_rad, "azimuth_rad").map_err(map_input_error)?;
if options.needs_carrier() {
validate::finite_positive(carrier_hz, "carrier_hz").map_err(map_input_error)?;
}
let epoch = if options.needs_instant() {
Some(media_instant(t_rx_j2000_s)?)
} else {
None
};
let ionex_epoch_j2000_s = if options.needs_ionex_epoch() {
Some(rounded_j2000_seconds(t_rx_j2000_s)?)
} else {
None
};
let troposphere_m = match options.troposphere {
Some(troposphere) => {
let epoch = epoch.expect("troposphere media requires an epoch");
let zenith = tropo_zenith(TropoModel::Saastamoinen, receiver, troposphere.met)
.map_err(map_media_error)?;
let mapping = tropo_mapping(troposphere.mapping, elevation_rad, receiver, epoch)
.map_err(map_media_error)?;
let delay_m = zenith.dry_m * mapping.dry + zenith.wet_m * mapping.wet;
validate::finite(delay_m, "media.troposphere_m").map_err(map_input_error)?;
delay_m
}
None => 0.0,
};
let ionosphere_m = match options.ionosphere {
Some(ObservableIonosphereCorrection::Broadcast(model)) => {
let epoch = epoch.expect("broadcast ionosphere media requires an epoch");
let delay_m = ionosphere_delay(
receiver,
elevation_rad,
azimuth_rad,
epoch,
carrier_hz,
&model,
)
.map_err(map_media_error)?;
validate::finite(delay_m, "media.ionosphere_m").map_err(map_input_error)?;
delay_m
}
Some(ObservableIonosphereCorrection::Ionex(ionex)) => {
let ionex_epoch_j2000_s =
ionex_epoch_j2000_s.expect("IONEX media requires an integer epoch");
let delay_m = ionex_slant_delay(
ionex,
receiver,
elevation_rad,
azimuth_rad,
ionex_epoch_j2000_s,
carrier_hz,
)
.map_err(map_media_error)?;
validate::finite(delay_m, "media.ionosphere_m").map_err(map_input_error)?;
delay_m
}
None => 0.0,
};
let total_m = troposphere_m + ionosphere_m;
validate::finite(total_m, "media.total_m").map_err(map_input_error)?;
Ok(AppliedMediaCorrections {
troposphere_m,
ionosphere_m,
total_m,
})
}
pub fn observable_states_at_j2000_s(
source: &dyn ObservableEphemerisSource,
satellites: &[GnssSatelliteId],
epochs_j2000_s: &[f64],
) -> Result<ObservableStateBatch, ObservablesError> {
source.observable_states_at_j2000_s(satellites, epochs_j2000_s)
}
pub fn observable_states_at_shared_j2000_s(
source: &dyn ObservableEphemerisSource,
satellites: &[GnssSatelliteId],
epoch_j2000_s: f64,
) -> ObservableStateBatch {
source.observable_states_at_shared_j2000_s(satellites, epoch_j2000_s)
}
pub fn transmit_time_satellite_state(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: TransmitTimeOptions,
) -> Result<TransmitTimeSatelliteState, ObservablesError> {
validate_transmit_time_inputs(receiver_ecef_m, t_rx_j2000_s)?;
let predict_options = PredictOptions {
carrier_hz: F_L1_HZ,
light_time: options.light_time,
sagnac: options.sagnac,
};
let solved = solve_transmit_time(source, sat, receiver_ecef_m, t_rx_j2000_s, predict_options)?;
let dx = solved.sat_rot_ecef_m[0] - receiver_ecef_m[0];
let dy = solved.sat_rot_ecef_m[1] - receiver_ecef_m[1];
let dz = solved.sat_rot_ecef_m[2] - receiver_ecef_m[2];
let range = geometric_range_m([dx, dy, dz])?;
let los = [dx / range, dy / range, dz / range];
let velocity = satellite_velocity(source, sat, solved.transmit_time_j2000_s)?;
let velocity_rot = sagnac_rotate(velocity, solved.tau_s, options.sagnac);
validate::finite_vec3(velocity_rot, "satellite velocity_m_s").map_err(map_input_error)?;
Ok(TransmitTimeSatelliteState {
signal_flight_time_s: solved.tau_s,
transmit_offset_us: solved.transmit_offset_us,
transmit_time_j2000_s: solved.transmit_time_j2000_s,
clock_s: solved.state.clock_s,
transmit_position_ecef_m: solved.state.position_ecef_m,
position_ecef_m: solved.sat_rot_ecef_m,
velocity_m_s: velocity_rot,
geometric_range_m: range,
los_unit: los,
})
}
pub fn predict(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: PredictOptions,
) -> Result<PredictedObservables, ObservablesError> {
let (prediction, _) = predict_core(source, sat, receiver_ecef_m, t_rx_j2000_s, options)?;
Ok(prediction)
}
pub fn predict_with_media(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: MediaPredictOptions<'_>,
) -> Result<MediaPredictedObservables, ObservablesError> {
let (prediction, topocentric) = predict_core(
source,
sat,
receiver_ecef_m,
t_rx_j2000_s,
options.prediction,
)?;
if options.media.is_disabled() {
return Ok(MediaPredictedObservables {
range_m: prediction.geometric_range_m,
prediction,
media: AppliedMediaCorrections::default(),
});
}
let media = observable_media_corrections(
topocentric.receiver,
topocentric.elevation_rad,
topocentric.azimuth_rad,
t_rx_j2000_s,
options.prediction.carrier_hz,
options.media,
)?;
let range_m = prediction.geometric_range_m + media.total_m;
validate::finite(range_m, "range_m").map_err(map_input_error)?;
Ok(MediaPredictedObservables {
prediction,
range_m,
media,
})
}
fn predict_core(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: PredictOptions,
) -> Result<(PredictedObservables, TopocentricGeometry), ObservablesError> {
validate_predict_inputs(receiver_ecef_m, t_rx_j2000_s, options)?;
let solved = solve_transmit_time(source, sat, receiver_ecef_m, t_rx_j2000_s, options)?;
let dx = solved.sat_rot_ecef_m[0] - receiver_ecef_m[0];
let dy = solved.sat_rot_ecef_m[1] - receiver_ecef_m[1];
let dz = solved.sat_rot_ecef_m[2] - receiver_ecef_m[2];
let range = geometric_range_m([dx, dy, dz])?;
let los = [dx / range, dy / range, dz / range];
let velocity = satellite_velocity(source, sat, solved.transmit_time_j2000_s)?;
let velocity_rot = sagnac_rotate(velocity, solved.tau_s, options.sagnac);
validate::finite_vec3(velocity_rot, "satellite velocity_m_s").map_err(map_input_error)?;
let range_rate = los[0] * velocity_rot[0] + los[1] * velocity_rot[1] + los[2] * velocity_rot[2];
validate::finite(range_rate, "range_rate_m_s").map_err(map_input_error)?;
let doppler_hz = -range_rate * options.carrier_hz / C_M_S;
validate::finite(doppler_hz, "doppler_hz").map_err(map_input_error)?;
let topocentric = topocentric(receiver_ecef_m, [dx, dy, dz], range)?;
Ok((
PredictedObservables {
geometric_range_m: range,
range_rate_m_s: range_rate,
doppler_hz,
sat_clock_s: solved.state.clock_s,
elevation_deg: topocentric.elevation_deg,
azimuth_deg: topocentric.azimuth_deg,
transmit_offset_us: solved.transmit_offset_us,
transmit_time_j2000_s: solved.transmit_time_j2000_s,
los_unit: los,
sat_pos_ecef_m: solved.sat_rot_ecef_m,
sat_velocity_m_s: velocity_rot,
},
topocentric,
))
}
pub type PredictRequest = (GnssSatelliteId, [f64; 3], f64);
pub fn predict_batch(
source: &dyn ObservableEphemerisSource,
requests: &[PredictRequest],
options: PredictOptions,
) -> Vec<Result<PredictedObservables, ObservablesError>> {
requests
.iter()
.map(|&(sat, receiver_ecef_m, t_rx_j2000_s)| {
predict(source, sat, receiver_ecef_m, t_rx_j2000_s, options)
})
.collect()
}
pub fn predict_batch_with_media(
source: &dyn ObservableEphemerisSource,
requests: &[PredictRequest],
options: MediaPredictOptions<'_>,
) -> Vec<Result<MediaPredictedObservables, ObservablesError>> {
requests
.iter()
.map(|&(sat, receiver_ecef_m, t_rx_j2000_s)| {
predict_with_media(source, sat, receiver_ecef_m, t_rx_j2000_s, options)
})
.collect()
}
pub fn predict_batch_parallel(
source: &(dyn ObservableEphemerisSource + Sync),
requests: &[PredictRequest],
options: PredictOptions,
) -> Vec<Result<PredictedObservables, ObservablesError>> {
requests
.par_iter()
.map(|&(sat, receiver_ecef_m, t_rx_j2000_s)| {
predict(source, sat, receiver_ecef_m, t_rx_j2000_s, options)
})
.collect()
}
pub fn predict_batch_with_media_parallel(
source: &(dyn ObservableEphemerisSource + Sync),
requests: &[PredictRequest],
options: MediaPredictOptions<'_>,
) -> Vec<Result<MediaPredictedObservables, ObservablesError>> {
requests
.par_iter()
.map(|&(sat, receiver_ecef_m, t_rx_j2000_s)| {
predict_with_media(source, sat, receiver_ecef_m, t_rx_j2000_s, options)
})
.collect()
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RangePredictionRequest {
pub sat: GnssSatelliteId,
pub receiver_ecef_m: [f64; 3],
pub t_rx_j2000_s: f64,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RangePrediction {
pub geometric_range_m: f64,
pub sat_clock_s: Option<f64>,
pub transmit_time_j2000_s: f64,
pub sat_pos_ecef_m: [f64; 3],
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MediaRangePrediction {
pub prediction: RangePrediction,
pub range_m: f64,
pub media: AppliedMediaCorrections,
}
pub fn predict_ranges(
source: &dyn ObservableEphemerisSource,
requests: &[RangePredictionRequest],
options: PredictOptions,
out: &mut [RangePrediction],
) -> Result<(), ObservablesError> {
if out.len() != requests.len() {
return Err(ObservablesError::InvalidInput {
field: "out",
kind: ObservablesInputErrorKind::OutOfRange,
});
}
for (request, slot) in requests.iter().zip(out.iter_mut()) {
*slot = range_prediction_at_rx(
source,
request.sat,
request.receiver_ecef_m,
request.t_rx_j2000_s,
options,
)?;
}
Ok(())
}
pub fn predict_ranges_with_media(
source: &dyn ObservableEphemerisSource,
requests: &[RangePredictionRequest],
options: MediaPredictOptions<'_>,
out: &mut [MediaRangePrediction],
) -> Result<(), ObservablesError> {
if out.len() != requests.len() {
return Err(ObservablesError::InvalidInput {
field: "out",
kind: ObservablesInputErrorKind::OutOfRange,
});
}
for (request, slot) in requests.iter().zip(out.iter_mut()) {
if options.media.is_disabled() {
let prediction = range_prediction_at_rx(
source,
request.sat,
request.receiver_ecef_m,
request.t_rx_j2000_s,
options.prediction,
)?;
*slot = MediaRangePrediction {
range_m: prediction.geometric_range_m,
prediction,
media: AppliedMediaCorrections::default(),
};
continue;
}
let (prediction, topocentric) = range_prediction_core(
source,
request.sat,
request.receiver_ecef_m,
request.t_rx_j2000_s,
options.prediction,
)?;
let media = observable_media_corrections(
topocentric.receiver,
topocentric.elevation_rad,
topocentric.azimuth_rad,
request.t_rx_j2000_s,
options.prediction.carrier_hz,
options.media,
)?;
let range_m = prediction.geometric_range_m + media.total_m;
validate::finite(range_m, "range_m").map_err(map_input_error)?;
*slot = MediaRangePrediction {
prediction,
range_m,
media,
};
}
Ok(())
}
fn range_prediction_at_rx(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: PredictOptions,
) -> Result<RangePrediction, ObservablesError> {
let (prediction, _, _) =
range_prediction_state(source, sat, receiver_ecef_m, t_rx_j2000_s, options)?;
Ok(prediction)
}
fn range_prediction_core(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: PredictOptions,
) -> Result<(RangePrediction, TopocentricGeometry), ObservablesError> {
let (prediction, line_of_sight_m, range) =
range_prediction_state(source, sat, receiver_ecef_m, t_rx_j2000_s, options)?;
let topocentric = topocentric(receiver_ecef_m, line_of_sight_m, range)?;
Ok((prediction, topocentric))
}
fn range_prediction_state(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: PredictOptions,
) -> Result<(RangePrediction, [f64; 3], f64), ObservablesError> {
validate_transmit_time_inputs(receiver_ecef_m, t_rx_j2000_s)?;
let solved = solve_transmit_time(source, sat, receiver_ecef_m, t_rx_j2000_s, options)?;
let dx = solved.sat_rot_ecef_m[0] - receiver_ecef_m[0];
let dy = solved.sat_rot_ecef_m[1] - receiver_ecef_m[1];
let dz = solved.sat_rot_ecef_m[2] - receiver_ecef_m[2];
let line_of_sight_m = [dx, dy, dz];
let range = geometric_range_m([dx, dy, dz])?;
Ok((
RangePrediction {
geometric_range_m: range,
sat_clock_s: solved.state.clock_s,
transmit_time_j2000_s: solved.transmit_time_j2000_s,
sat_pos_ecef_m: solved.sat_rot_ecef_m,
},
line_of_sight_m,
range,
))
}
#[derive(Debug, Clone, Copy)]
struct SolvedTransmitTime {
tau_s: f64,
transmit_offset_us: i64,
transmit_time_j2000_s: f64,
state: ObservableState,
sat_rot_ecef_m: [f64; 3],
}
fn solve_transmit_time(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: PredictOptions,
) -> Result<SolvedTransmitTime, ObservablesError> {
if !options.light_time {
let state = validated_state_at_j2000_s(source, sat, t_rx_j2000_s)?;
let sat_rot = sagnac_rotate(state.position_ecef_m, 0.0, options.sagnac);
validate::finite_vec3(sat_rot, "satellite position_ecef_m").map_err(map_input_error)?;
return Ok(SolvedTransmitTime {
tau_s: 0.0,
transmit_offset_us: 0,
transmit_time_j2000_s: t_rx_j2000_s,
state,
sat_rot_ecef_m: sat_rot,
});
}
let mut tau = 0.0;
for iter in 0..OBSERVABLE_TRANSMIT_TIME_ITERATIONS {
let transmit_offset_us = microseconds_from_tau(tau);
let t_tx = t_rx_j2000_s - transmit_offset_us as f64 / MICROSECONDS_PER_SECOND;
let state = validated_state_at_j2000_s(source, sat, t_tx)?;
let sat_rot = sagnac_rotate(state.position_ecef_m, tau, options.sagnac);
validate::finite_vec3(sat_rot, "satellite position_ecef_m").map_err(map_input_error)?;
let dx = sat_rot[0] - receiver_ecef_m[0];
let dy = sat_rot[1] - receiver_ecef_m[1];
let dz = sat_rot[2] - receiver_ecef_m[2];
let range = geometric_range_m([dx, dy, dz])?;
let new_tau = range / C_M_S;
if iter + 1 == OBSERVABLE_TRANSMIT_TIME_ITERATIONS {
return finalize_transmit_time(source, sat, t_rx_j2000_s, new_tau, options.sagnac);
}
tau = new_tau;
}
unreachable!("fixed transmit-time loop always returns on its last iteration")
}
fn finalize_transmit_time(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
t_rx_j2000_s: f64,
tau: f64,
sagnac: bool,
) -> Result<SolvedTransmitTime, ObservablesError> {
let transmit_offset_us = microseconds_from_tau(tau);
let t_tx = t_rx_j2000_s - transmit_offset_us as f64 / MICROSECONDS_PER_SECOND;
validate::finite(t_tx, "transmit_time_j2000_s").map_err(map_input_error)?;
let state = validated_state_at_j2000_s(source, sat, t_tx)?;
let sat_rot = sagnac_rotate(state.position_ecef_m, tau, sagnac);
validate::finite_vec3(sat_rot, "satellite position_ecef_m").map_err(map_input_error)?;
Ok(SolvedTransmitTime {
tau_s: tau,
transmit_offset_us,
transmit_time_j2000_s: t_tx,
state,
sat_rot_ecef_m: sat_rot,
})
}
fn microseconds_from_tau(tau_s: f64) -> i64 {
(tau_s * MICROSECONDS_PER_SECOND).round() as i64
}
fn satellite_velocity(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
t_tx_j2000_s: f64,
) -> Result<[f64; 3], ObservablesError> {
let plus = validated_state_at_j2000_s(source, sat, t_tx_j2000_s + FD_HALF_S)?;
let minus = validated_state_at_j2000_s(source, sat, t_tx_j2000_s - FD_HALF_S)?;
let denom = 2.0 * FD_HALF_S;
let velocity = [
(plus.position_ecef_m[0] - minus.position_ecef_m[0]) / denom,
(plus.position_ecef_m[1] - minus.position_ecef_m[1]) / denom,
(plus.position_ecef_m[2] - minus.position_ecef_m[2]) / denom,
];
validate::finite_vec3(velocity, "satellite velocity_m_s").map_err(map_input_error)
}
fn validate_predict_inputs(
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
options: PredictOptions,
) -> Result<(), ObservablesError> {
validate::finite_vec3(receiver_ecef_m, "receiver_ecef_m").map_err(map_input_error)?;
validate::finite(t_rx_j2000_s, "t_rx_j2000_s").map_err(map_input_error)?;
validate::finite_positive(options.carrier_hz, "options.carrier_hz").map_err(map_input_error)?;
Ok(())
}
fn validate_transmit_time_inputs(
receiver_ecef_m: [f64; 3],
t_rx_j2000_s: f64,
) -> Result<(), ObservablesError> {
validate::finite_vec3(receiver_ecef_m, "receiver_ecef_m").map_err(map_input_error)?;
validate::finite(t_rx_j2000_s, "t_rx_j2000_s").map_err(map_input_error)?;
Ok(())
}
fn validated_state_at_j2000_s(
source: &dyn ObservableEphemerisSource,
sat: GnssSatelliteId,
t_j2000_s: f64,
) -> Result<ObservableState, ObservablesError> {
let state = source.observable_state_at_j2000_s(sat, t_j2000_s)?;
validate_observable_state(&state)?;
Ok(state)
}
fn validate_observable_state(state: &ObservableState) -> Result<(), ObservablesError> {
validate::finite_vec3(state.position_ecef_m, "observable state position_ecef_m")
.map_err(map_input_error)?;
if let Some(clock_s) = state.clock_s {
validate::finite(clock_s, "observable state clock_s").map_err(map_input_error)?;
}
Ok(())
}
fn geometric_range_m(delta_ecef_m: [f64; 3]) -> Result<f64, ObservablesError> {
let range = (delta_ecef_m[0] * delta_ecef_m[0]
+ delta_ecef_m[1] * delta_ecef_m[1]
+ delta_ecef_m[2] * delta_ecef_m[2])
.sqrt();
validate::finite_positive(range, "geometric_range_m").map_err(map_input_error)
}
fn map_input_error(error: validate::FieldError) -> ObservablesError {
ObservablesError::InvalidInput {
field: error.field(),
kind: ObservablesInputErrorKind::from(&error),
}
}
fn invalid_observable_input(
field: &'static str,
kind: ObservablesInputErrorKind,
) -> ObservablesError {
ObservablesError::InvalidInput { field, kind }
}
fn media_instant(t_rx_j2000_s: f64) -> Result<Instant, ObservablesError> {
validate::finite(t_rx_j2000_s, "t_rx_j2000_s").map_err(map_input_error)?;
let days = (t_rx_j2000_s / SECONDS_PER_DAY).floor();
let seconds_into_day = t_rx_j2000_s - days * SECONDS_PER_DAY;
let fraction = seconds_into_day / SECONDS_PER_DAY;
let split = JulianDateSplit::new(J2000_JD + days, fraction).map_err(|_| {
invalid_observable_input("t_rx_j2000_s", ObservablesInputErrorKind::OutOfRange)
})?;
Ok(Instant::from_julian_date(TimeScale::Gpst, split))
}
fn rounded_j2000_seconds(t_rx_j2000_s: f64) -> Result<i64, ObservablesError> {
validate::finite(t_rx_j2000_s, "t_rx_j2000_s").map_err(map_input_error)?;
let rounded = t_rx_j2000_s.round();
if !rounded.is_finite() || rounded < i64::MIN as f64 || rounded > i64::MAX as f64 {
return Err(invalid_observable_input(
"t_rx_j2000_s",
ObservablesInputErrorKind::OutOfRange,
));
}
Ok(rounded as i64)
}
fn map_media_error(error: Error) -> ObservablesError {
match error {
Error::InvalidInput(message) => map_media_invalid_input(&message),
_ => invalid_observable_input("media", ObservablesInputErrorKind::OutOfRange),
}
}
fn map_media_invalid_input(message: &str) -> ObservablesError {
let kind = if message.ends_with("not finite") {
ObservablesInputErrorKind::NonFinite
} else if message.ends_with("not positive") {
ObservablesInputErrorKind::NotPositive
} else if message.ends_with("negative") {
ObservablesInputErrorKind::Negative
} else {
ObservablesInputErrorKind::OutOfRange
};
let field = if message.starts_with("elevation_rad ") {
"media.elevation_rad"
} else if message.starts_with("receiver.lat_rad ") {
"media.receiver.lat_rad"
} else if message.starts_with("receiver.lon_rad ") {
"media.receiver.lon_rad"
} else if message.starts_with("receiver.height_m ") {
"media.receiver.height_m"
} else if message.starts_with("pressure_hpa ") {
"media.pressure_hpa"
} else if message.starts_with("temperature_k ") {
"media.temperature_k"
} else if message.starts_with("relative_humidity ") {
"media.relative_humidity"
} else if message.starts_with("frequency_hz ") {
"media.carrier_hz"
} else if message.starts_with("azimuth_rad ") {
"media.azimuth_rad"
} else {
"media"
};
invalid_observable_input(field, kind)
}
fn sagnac_rotate(pos: [f64; 3], tau_s: f64, apply: bool) -> [f64; 3] {
let sagnac = if apply {
SagnacRecipe::ClosedFormZRotation
} else {
SagnacRecipe::Off
};
crate::estimation::substrate::range::rotate_transmit_satellite(
sagnac,
pos,
tau_s,
OMEGA_E_DOT_RAD_S,
)
}
#[derive(Debug, Clone, Copy, PartialEq)]
struct TopocentricGeometry {
receiver: Wgs84Geodetic,
elevation_rad: f64,
azimuth_rad: f64,
elevation_deg: f64,
azimuth_deg: f64,
}
fn topocentric(
receiver_ecef_m: [f64; 3],
delta_ecef_m: [f64; 3],
range_m: f64,
) -> Result<TopocentricGeometry, ObservablesError> {
let (lat_deg, lon_deg, height_km) = itrs_to_geodetic_compute(
receiver_ecef_m[0] / KM_TO_M,
receiver_ecef_m[1] / KM_TO_M,
receiver_ecef_m[2] / KM_TO_M,
)
.map_err(|_| ObservablesError::InvalidInput {
field: "receiver_ecef_m",
kind: ObservablesInputErrorKind::OutOfRange,
})?;
let lat = lat_deg * PI / DEGREES_PER_SEMICIRCLE;
let lon = lon_deg * PI / DEGREES_PER_SEMICIRCLE;
let receiver = Wgs84Geodetic::new(lat, lon, height_km * KM_TO_M).map_err(|_| {
ObservablesError::InvalidInput {
field: "receiver_ecef_m",
kind: ObservablesInputErrorKind::OutOfRange,
}
})?;
let sl = lat.sin();
let cl = lat.cos();
let so = lon.sin();
let co = lon.cos();
let dx = delta_ecef_m[0];
let dy = delta_ecef_m[1];
let dz = delta_ecef_m[2];
let e = -so * dx + co * dy;
let n = -sl * co * dx - sl * so * dy + cl * dz;
let u = cl * co * dx + cl * so * dy + sl * dz;
let horiz_sq = e * e + n * n;
let (azimuth_rad, mut azimuth_deg) = if horiz_sq < AZIMUTH_ZENITH_EPS * range_m * range_m {
(0.0, 0.0)
} else {
let raw_azimuth_rad = e.atan2(n);
(
if raw_azimuth_rad < 0.0 {
raw_azimuth_rad + 2.0 * PI
} else {
raw_azimuth_rad
},
raw_azimuth_rad * DEGREES_PER_SEMICIRCLE / PI,
)
};
if azimuth_deg < 0.0 {
azimuth_deg += DEGREES_PER_CIRCLE;
}
let sin_elevation = (u / range_m).clamp(-1.0, 1.0);
let elevation_rad = sin_elevation.asin();
let elevation_deg = elevation_rad * DEGREES_PER_SEMICIRCLE / PI;
validate::finite(elevation_rad, "elevation_rad").map_err(map_input_error)?;
validate::finite(elevation_deg, "elevation_deg").map_err(map_input_error)?;
validate::finite(azimuth_rad, "azimuth_rad").map_err(map_input_error)?;
validate::finite(azimuth_deg, "azimuth_deg").map_err(map_input_error)?;
Ok(TopocentricGeometry {
receiver,
elevation_rad,
azimuth_rad,
elevation_deg,
azimuth_deg,
})
}
#[cfg(test)]
mod public_api_tests {
use super::*;
use crate::{GnssSatelliteId, GnssSystem};
#[derive(Debug, Clone, Copy)]
struct StaticSource {
state: ObservableState,
}
impl ObservableEphemerisSource for StaticSource {
fn observable_state_at_j2000_s(
&self,
_sat: GnssSatelliteId,
_t_j2000_s: f64,
) -> Result<ObservableState, ObservablesError> {
Ok(self.state)
}
}
#[test]
fn predict_ranges_matches_transmit_time_loop_bitwise() {
let source = StaticSource {
state: ObservableState {
position_ecef_m: [20_200_000.0, 14_000_000.0, 21_700_000.0],
clock_s: Some(1.25e-6),
},
};
let options = PredictOptions {
carrier_hz: F_L1_HZ,
light_time: true,
sagnac: true,
};
let sat1 = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let sat2 = GnssSatelliteId::new(GnssSystem::Gps, 7).expect("valid satellite id");
let requests = [
RangePredictionRequest {
sat: sat1,
receiver_ecef_m: [4_027_894.0, 307_046.0, 4_919_474.0],
t_rx_j2000_s: 646_272_000.0,
},
RangePredictionRequest {
sat: sat2,
receiver_ecef_m: [1_130_000.0, -4_830_000.0, 3_994_000.0],
t_rx_j2000_s: 646_272_060.0,
},
];
let mut out = [RangePrediction {
geometric_range_m: 0.0,
sat_clock_s: None,
transmit_time_j2000_s: 0.0,
sat_pos_ecef_m: [0.0; 3],
}; 2];
predict_ranges(&source, &requests, options, &mut out).expect("batch range prediction");
let tt_options = TransmitTimeOptions {
light_time: options.light_time,
sagnac: options.sagnac,
};
for (request, got) in requests.iter().zip(out.iter()) {
let single = transmit_time_satellite_state(
&source,
request.sat,
request.receiver_ecef_m,
request.t_rx_j2000_s,
tt_options,
)
.expect("single transmit-time state");
assert_eq!(
got.geometric_range_m.to_bits(),
single.geometric_range_m.to_bits()
);
assert_eq!(
got.transmit_time_j2000_s.to_bits(),
single.transmit_time_j2000_s.to_bits()
);
assert_eq!(
got.sat_clock_s.map(f64::to_bits),
single.clock_s.map(f64::to_bits)
);
assert_eq!(
got.sat_pos_ecef_m.map(f64::to_bits),
single.position_ecef_m.map(f64::to_bits)
);
}
}
#[test]
fn predict_ranges_batch_matches_scalar_calls_bitwise() {
let source = StaticSource {
state: ObservableState {
position_ecef_m: [20_200_000.0, 14_000_000.0, 21_700_000.0],
clock_s: Some(1.25e-6),
},
};
let options = PredictOptions::default();
let sat1 = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let sat2 = GnssSatelliteId::new(GnssSystem::Gps, 7).expect("valid satellite id");
let requests = [
RangePredictionRequest {
sat: sat1,
receiver_ecef_m: [4_027_894.0, 307_046.0, 4_919_474.0],
t_rx_j2000_s: 646_272_000.0,
},
RangePredictionRequest {
sat: sat2,
receiver_ecef_m: [1_130_000.0, -4_830_000.0, 3_994_000.0],
t_rx_j2000_s: 646_272_060.0,
},
RangePredictionRequest {
sat: sat1,
receiver_ecef_m: [-2_700_000.0, -4_290_000.0, 3_855_000.0],
t_rx_j2000_s: 646_272_120.0,
},
];
let zero = RangePrediction {
geometric_range_m: 0.0,
sat_clock_s: None,
transmit_time_j2000_s: 0.0,
sat_pos_ecef_m: [0.0; 3],
};
let mut batch = [zero; 3];
predict_ranges(&source, &requests, options, &mut batch).expect("batch ranges");
for (i, request) in requests.iter().enumerate() {
let mut single = [zero; 1];
predict_ranges(&source, std::slice::from_ref(request), options, &mut single)
.expect("single range");
assert_eq!(
batch[i].geometric_range_m.to_bits(),
single[0].geometric_range_m.to_bits()
);
assert_eq!(
batch[i].transmit_time_j2000_s.to_bits(),
single[0].transmit_time_j2000_s.to_bits()
);
assert_eq!(
batch[i].sat_clock_s.map(f64::to_bits),
single[0].sat_clock_s.map(f64::to_bits)
);
assert_eq!(
batch[i].sat_pos_ecef_m.map(f64::to_bits),
single[0].sat_pos_ecef_m.map(f64::to_bits)
);
}
}
#[test]
fn predict_ranges_rejects_length_mismatch() {
let source = StaticSource {
state: ObservableState {
position_ecef_m: [20_200_000.0, 14_000_000.0, 21_700_000.0],
clock_s: None,
},
};
let sat = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let requests = [RangePredictionRequest {
sat,
receiver_ecef_m: [4_027_894.0, 307_046.0, 4_919_474.0],
t_rx_j2000_s: 646_272_000.0,
}];
let mut out: [RangePrediction; 0] = [];
let err = predict_ranges(&source, &requests, PredictOptions::default(), &mut out)
.expect_err("length mismatch must fail");
match err {
ObservablesError::InvalidInput { field, kind } => {
assert_eq!(field, "out");
assert_eq!(kind, ObservablesInputErrorKind::OutOfRange);
}
other => panic!("expected InvalidInput(out, OutOfRange), got {other:?}"),
}
}
#[test]
fn topocentric_elevation_is_ninety_at_non_equatorial_zenith() {
let rx = [
4_509_179.095_483_66,
275_556.225_682_215_9,
4_487_348.408_865_919,
];
let (lat_deg, lon_deg, _h) =
itrs_to_geodetic_compute(rx[0] / KM_TO_M, rx[1] / KM_TO_M, rx[2] / KM_TO_M)
.expect("receiver geodetic conversion");
assert!(lat_deg.abs() > 40.0, "receiver must be non-equatorial");
let lat = lat_deg * PI / DEGREES_PER_SEMICIRCLE;
let lon = lon_deg * PI / DEGREES_PER_SEMICIRCLE;
let (sl, cl) = lat.sin_cos();
let (so, co) = lon.sin_cos();
let up = [cl * co, cl * so, sl];
let d = 20_000_000.0_f64;
let delta = [up[0] * d, up[1] * d, up[2] * d];
let range = (delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]).sqrt();
let u = cl * co * delta[0] + cl * so * delta[1] + sl * delta[2];
assert!(
u / range > 1.0,
"test geometry must overshoot the asin domain"
);
let geometry = topocentric(rx, delta, range).expect("non-equatorial zenith must not error");
assert!(geometry.elevation_deg.is_finite());
assert!((geometry.elevation_deg - 90.0).abs() < 1e-9);
}
#[test]
fn transmit_time_state_matches_predict_substrate_with_no_light_time() {
let source = StaticSource {
state: ObservableState {
position_ecef_m: [20_200_000.0, 14_000_000.0, 21_700_000.0],
clock_s: Some(1.25e-6),
},
};
let sat = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let rx = [4_027_894.0, 307_046.0, 4_919_474.0];
let state = transmit_time_satellite_state(
&source,
sat,
rx,
646_272_000.0,
TransmitTimeOptions {
light_time: false,
sagnac: true,
},
)
.expect("state");
let prediction = predict(
&source,
sat,
rx,
646_272_000.0,
PredictOptions {
carrier_hz: F_L1_HZ,
light_time: false,
sagnac: true,
},
)
.expect("prediction");
assert_eq!(state.signal_flight_time_s.to_bits(), 0.0f64.to_bits());
assert_eq!(state.transmit_offset_us, 0);
assert_eq!(
state.transmit_time_j2000_s.to_bits(),
646_272_000.0f64.to_bits()
);
assert_eq!(state.clock_s.unwrap().to_bits(), 1.25e-6f64.to_bits());
assert_eq!(
state.transmit_position_ecef_m.map(f64::to_bits),
source.state.position_ecef_m.map(f64::to_bits)
);
assert_eq!(
state.position_ecef_m.map(f64::to_bits),
prediction.sat_pos_ecef_m.map(f64::to_bits)
);
assert_eq!(
state.velocity_m_s.map(f64::to_bits),
prediction.sat_velocity_m_s.map(f64::to_bits)
);
assert_eq!(
state.geometric_range_m.to_bits(),
prediction.geometric_range_m.to_bits()
);
assert_eq!(
state.los_unit.map(f64::to_bits),
prediction.los_unit.map(f64::to_bits)
);
}
}
#[cfg(test)]
mod media_validation_tests {
use super::*;
use crate::astro::time::civil::split_julian_date_from_j2000_seconds;
use crate::ionex::TecGridSamples;
use crate::GnssSystem;
const T_RX_J2000_S: f64 = 646_272_000.0;
const T_RX_J2000_I64: i64 = 646_272_000;
#[derive(Debug, Clone, Copy)]
struct StaticSource {
state: ObservableState,
}
impl ObservableEphemerisSource for StaticSource {
fn observable_state_at_j2000_s(
&self,
_sat: GnssSatelliteId,
_t_j2000_s: f64,
) -> Result<ObservableState, ObservablesError> {
Ok(self.state)
}
}
fn epoch() -> Instant {
let (jd_whole, fraction) = split_julian_date_from_j2000_seconds(T_RX_J2000_I64);
Instant::from_julian_date(
TimeScale::Gpst,
JulianDateSplit::new(jd_whole, fraction).expect("valid media epoch"),
)
}
fn receiver() -> Wgs84Geodetic {
Wgs84Geodetic::new(0.0, 0.0, 0.0).expect("valid receiver")
}
fn met() -> Met {
Met::new(1013.25, 288.15, 0.5).expect("valid met")
}
fn klobuchar_model() -> IonoModel {
IonoModel::Klobuchar(crate::ionex::KlobucharParams {
alpha: [0.0; 4],
beta: [0.0; 4],
})
}
fn ionex() -> Ionex {
let map = vec![
vec![12.0, 12.0, 12.0],
vec![12.0, 12.0, 12.0],
vec![12.0, 12.0, 12.0],
];
Ionex::from_samples(TecGridSamples {
map_epochs: vec![epoch()],
lat_nodes_deg: vec![90.0, 0.0, -90.0],
lon_nodes_deg: vec![-180.0, 0.0, 180.0],
dlat_deg: -90.0,
dlon_deg: 180.0,
shell_height_km: 450.0,
base_radius_km: 6371.0,
exponent: 0,
tec_maps: vec![map],
rms_maps: Vec::new(),
})
.expect("valid IONEX samples")
}
fn direct_troposphere(elevation_rad: f64) -> f64 {
let zenith =
tropo_zenith(TropoModel::Saastamoinen, receiver(), met()).expect("zenith delay");
let mapping = tropo_mapping(MappingModel::Niell, elevation_rad, receiver(), epoch())
.expect("mapping");
zenith.dry_m * mapping.dry + zenith.wet_m * mapping.wet
}
fn assert_bits_eq(label: &str, got: f64, expected: f64) {
assert_eq!(
got.to_bits(),
expected.to_bits(),
"{label}: got {got:?}, expected {expected:?}"
);
}
fn assert_prediction_bits_eq(got: &PredictedObservables, expected: &PredictedObservables) {
assert_bits_eq(
"geometric range",
got.geometric_range_m,
expected.geometric_range_m,
);
assert_bits_eq("range-rate", got.range_rate_m_s, expected.range_rate_m_s);
assert_bits_eq("Doppler", got.doppler_hz, expected.doppler_hz);
assert_eq!(
got.sat_clock_s.map(f64::to_bits),
expected.sat_clock_s.map(f64::to_bits)
);
assert_bits_eq("elevation", got.elevation_deg, expected.elevation_deg);
assert_bits_eq("azimuth", got.azimuth_deg, expected.azimuth_deg);
assert_eq!(got.transmit_offset_us, expected.transmit_offset_us);
assert_bits_eq(
"transmit time",
got.transmit_time_j2000_s,
expected.transmit_time_j2000_s,
);
for k in 0..3 {
assert_bits_eq("los", got.los_unit[k], expected.los_unit[k]);
assert_bits_eq(
"satellite position",
got.sat_pos_ecef_m[k],
expected.sat_pos_ecef_m[k],
);
assert_bits_eq(
"satellite velocity",
got.sat_velocity_m_s[k],
expected.sat_velocity_m_s[k],
);
}
}
fn assert_range_prediction_bits_eq(got: &RangePrediction, expected: &RangePrediction) {
assert_bits_eq(
"range geometric",
got.geometric_range_m,
expected.geometric_range_m,
);
assert_eq!(
got.sat_clock_s.map(f64::to_bits),
expected.sat_clock_s.map(f64::to_bits)
);
assert_bits_eq(
"range transmit time",
got.transmit_time_j2000_s,
expected.transmit_time_j2000_s,
);
for k in 0..3 {
assert_bits_eq(
"range satellite position",
got.sat_pos_ecef_m[k],
expected.sat_pos_ecef_m[k],
);
}
}
#[test]
fn media_corrections_match_direct_tropo_and_klobuchar_bits() {
for elevation_deg in [5.0_f64, 15.0, 90.0] {
let elevation_rad = elevation_deg * PI / DEGREES_PER_SEMICIRCLE;
let azimuth_rad = 0.25;
let options = ObservableMediaOptions {
troposphere: Some(ObservableTroposphereCorrection {
met: met(),
mapping: MappingModel::Niell,
}),
ionosphere: Some(ObservableIonosphereCorrection::Broadcast(klobuchar_model())),
};
let got = observable_media_corrections(
receiver(),
elevation_rad,
azimuth_rad,
T_RX_J2000_S,
F_L1_HZ,
options,
)
.expect("media corrections");
let expected_tropo = direct_troposphere(elevation_rad);
let expected_iono = ionosphere_delay(
receiver(),
elevation_rad,
azimuth_rad,
epoch(),
F_L1_HZ,
&klobuchar_model(),
)
.expect("direct Klobuchar");
assert_bits_eq("troposphere", got.troposphere_m, expected_tropo);
assert_bits_eq("Klobuchar", got.ionosphere_m, expected_iono);
assert_bits_eq("total", got.total_m, expected_tropo + expected_iono);
}
}
#[test]
fn media_corrections_match_direct_ionex_bits() {
let ionex = ionex();
for elevation_deg in [5.0_f64, 15.0, 90.0] {
let elevation_rad = elevation_deg * PI / DEGREES_PER_SEMICIRCLE;
let azimuth_rad = 1.0;
let got = observable_media_corrections(
receiver(),
elevation_rad,
azimuth_rad,
T_RX_J2000_S,
F_L1_HZ,
ObservableMediaOptions {
troposphere: None,
ionosphere: Some(ObservableIonosphereCorrection::Ionex(&ionex)),
},
)
.expect("IONEX media correction");
let expected = ionex_slant_delay(
&ionex,
receiver(),
elevation_rad,
azimuth_rad,
T_RX_J2000_I64,
F_L1_HZ,
)
.expect("direct IONEX");
assert_bits_eq("IONEX", got.ionosphere_m, expected);
assert_bits_eq("IONEX total", got.total_m, expected);
}
}
#[test]
fn default_media_prediction_matches_predict_bits() {
let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid satellite id");
let rx = [6_378_137.0, 0.0, 0.0];
let source = StaticSource {
state: ObservableState {
position_ecef_m: [26_378_137.0, 0.0, 0.0],
clock_s: Some(0.0),
},
};
let options = PredictOptions {
carrier_hz: F_L1_HZ,
light_time: false,
sagnac: false,
};
let plain = predict(&source, sat, rx, T_RX_J2000_S, options).expect("plain predict");
let media = predict_with_media(
&source,
sat,
rx,
T_RX_J2000_S,
MediaPredictOptions {
prediction: options,
media: ObservableMediaOptions::default(),
},
)
.expect("default media predict");
assert_prediction_bits_eq(&media.prediction, &plain);
assert_bits_eq("default range", media.range_m, plain.geometric_range_m);
assert_eq!(media.media, AppliedMediaCorrections::default());
}
#[test]
fn default_media_prediction_skips_media_epoch_for_large_epoch() {
let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid satellite id");
let rx = [6_378_137.0, 0.0, 0.0];
let source = StaticSource {
state: ObservableState {
position_ecef_m: [26_378_137.0, 0.0, 0.0],
clock_s: Some(0.0),
},
};
let options = PredictOptions {
carrier_hz: F_L1_HZ,
light_time: false,
sagnac: false,
};
let t_rx = 1.0e20;
let plain = predict(&source, sat, rx, t_rx, options).expect("plain predict");
let media = predict_with_media(
&source,
sat,
rx,
t_rx,
MediaPredictOptions {
prediction: options,
media: ObservableMediaOptions::default(),
},
)
.expect("default media predict");
assert_prediction_bits_eq(&media.prediction, &plain);
assert_bits_eq("default range", media.range_m, plain.geometric_range_m);
assert_eq!(media.media, AppliedMediaCorrections::default());
}
#[test]
fn below_troposphere_validity_returns_typed_error() {
let err = observable_media_corrections(
receiver(),
2.0 * PI / DEGREES_PER_SEMICIRCLE,
0.0,
T_RX_J2000_S,
F_L1_HZ,
ObservableMediaOptions {
troposphere: Some(ObservableTroposphereCorrection::default()),
ionosphere: None,
},
)
.expect_err("below mapping validity must fail");
match err {
ObservablesError::InvalidInput { field, kind } => {
assert_eq!(field, "media.elevation_rad");
assert_eq!(kind, ObservablesInputErrorKind::OutOfRange);
}
other => panic!("expected typed InvalidInput, got {other:?}"),
}
}
#[test]
fn range_media_prediction_adds_direct_troposphere_bits() {
let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid satellite id");
let rx = [6_378_137.0, 0.0, 0.0];
let elevation_rad = core::f64::consts::FRAC_PI_2;
let range_m = 20_000_000.0;
let delta = [range_m, 0.0, 0.0];
let source = StaticSource {
state: ObservableState {
position_ecef_m: [rx[0] + delta[0], rx[1] + delta[1], rx[2] + delta[2]],
clock_s: Some(0.0),
},
};
let options = MediaPredictOptions {
prediction: PredictOptions {
carrier_hz: f64::NAN,
light_time: false,
sagnac: false,
},
media: ObservableMediaOptions {
troposphere: Some(ObservableTroposphereCorrection::default()),
ionosphere: None,
},
};
let request = [RangePredictionRequest {
sat,
receiver_ecef_m: rx,
t_rx_j2000_s: T_RX_J2000_S,
}];
let zero_prediction = RangePrediction {
geometric_range_m: 0.0,
sat_clock_s: None,
transmit_time_j2000_s: 0.0,
sat_pos_ecef_m: [0.0; 3],
};
let mut out = [MediaRangePrediction {
prediction: zero_prediction,
range_m: 0.0,
media: AppliedMediaCorrections::default(),
}];
predict_ranges_with_media(&source, &request, options, &mut out)
.expect("range media prediction");
let got = out[0];
let expected = got.prediction.geometric_range_m + direct_troposphere(elevation_rad);
assert_bits_eq("corrected range", got.range_m, expected);
}
#[test]
fn default_range_media_prediction_matches_range_bits_with_unused_carrier() {
let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid satellite id");
let rx = [6_378_137.0, 0.0, 0.0];
let source = StaticSource {
state: ObservableState {
position_ecef_m: [26_378_137.0, 0.0, 0.0],
clock_s: Some(0.0),
},
};
let options = PredictOptions {
carrier_hz: f64::NAN,
light_time: false,
sagnac: false,
};
let request = [RangePredictionRequest {
sat,
receiver_ecef_m: rx,
t_rx_j2000_s: T_RX_J2000_S,
}];
let zero_prediction = RangePrediction {
geometric_range_m: 0.0,
sat_clock_s: None,
transmit_time_j2000_s: 0.0,
sat_pos_ecef_m: [0.0; 3],
};
let mut plain = [zero_prediction];
predict_ranges(&source, &request, options, &mut plain).expect("plain range");
let mut media = [MediaRangePrediction {
prediction: zero_prediction,
range_m: 0.0,
media: AppliedMediaCorrections::default(),
}];
predict_ranges_with_media(
&source,
&request,
MediaPredictOptions {
prediction: options,
media: ObservableMediaOptions::default(),
},
&mut media,
)
.expect("default media range");
assert_range_prediction_bits_eq(&media[0].prediction, &plain[0]);
assert_bits_eq(
"default range",
media[0].range_m,
plain[0].geometric_range_m,
);
assert_eq!(media[0].media, AppliedMediaCorrections::default());
}
}
#[cfg(all(test, sidereon_repo_tests))]
mod tests {
use super::*;
use crate::{GnssSatelliteId, GnssSystem};
#[derive(Debug, Clone, Copy)]
struct StaticSource {
state: ObservableState,
}
impl ObservableEphemerisSource for StaticSource {
fn observable_state_at_j2000_s(
&self,
_sat: GnssSatelliteId,
_t_j2000_s: f64,
) -> Result<ObservableState, ObservablesError> {
Ok(self.state)
}
}
fn sp3_fixture() -> Sp3 {
let path = concat!(
env!("CARGO_MANIFEST_DIR"),
"/tests/fixtures/sp3/GRG0MGXFIN_20201760000_01D_15M_ORB.SP3"
);
let bytes = std::fs::read(path).unwrap_or_else(|e| panic!("read SP3 fixture {path}: {e}"));
Sp3::parse(&bytes).expect("parse SP3 fixture")
}
fn static_source(position_ecef_m: [f64; 3]) -> StaticSource {
StaticSource {
state: ObservableState {
position_ecef_m,
clock_s: Some(0.0),
},
}
}
fn no_light_time_options() -> PredictOptions {
PredictOptions {
carrier_hz: F_L1_HZ,
light_time: false,
sagnac: true,
}
}
fn assert_invalid_observables_input(
err: ObservablesError,
field: &'static str,
kind: ObservablesInputErrorKind,
) {
match err {
ObservablesError::InvalidInput {
field: got_field,
kind: got_kind,
} => {
assert_eq!(got_field, field);
assert_eq!(got_kind, kind);
}
other => panic!("expected InvalidInput({field}, {kind:?}), got {other:?}"),
}
}
#[test]
fn split_julian_to_j2000_seconds_matches_orbis_time() {
let t = j2000_seconds_from_split(2_459_024.5, 0.5).expect("valid split Julian date");
assert_eq!(t, 646_272_000.0);
}
#[test]
fn split_julian_to_j2000_seconds_rejects_non_finite_parts() {
for (jd_whole, jd_fraction, field) in [
(f64::NAN, 0.5, "jd_whole"),
(f64::INFINITY, 0.5, "jd_whole"),
(2_459_024.5, f64::NAN, "jd_fraction"),
(2_459_024.5, f64::NEG_INFINITY, "jd_fraction"),
] {
let err = j2000_seconds_from_split(jd_whole, jd_fraction)
.expect_err("non-finite split Julian date part must fail");
assert_invalid_observables_input(err, field, ObservablesInputErrorKind::NonFinite);
}
}
#[test]
fn sp3_predict_reference_case() {
let sp3 = sp3_fixture();
let sat = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let rx = [3_512_900.0, 780_500.0, 5_248_700.0];
let obs = predict(&sp3, sat, rx, 646_272_000.0, PredictOptions::default())
.expect("predict observables");
assert_eq!(obs.geometric_range_m.to_bits(), 0x4173cf438ba57358);
assert_eq!(obs.range_rate_m_s.to_bits(), 0x402d7dd36f6b8980);
assert_eq!(obs.doppler_hz.to_bits(), 0xc0535f534ba7c77d);
assert_eq!(obs.sat_clock_s.unwrap().to_bits(), 0x3ef04d2d8279460c);
assert_eq!(obs.elevation_deg.to_bits(), 0x4054590eed870f52);
assert_eq!(obs.azimuth_deg.to_bits(), 0x40645ff5a090a131);
assert_eq!(obs.transmit_offset_us, 69_288);
assert_eq!(obs.transmit_time_j2000_s.to_bits(), 0x41c342a9fff72192);
assert_eq!(
obs.los_unit.map(f64::to_bits),
[0x3fe4c70da9fa70dd, 0x3fc834429adb2bae, 0x3fe792a4f57fdcb1,]
);
assert_eq!(
obs.sat_pos_ecef_m.map(f64::to_bits),
[0x41703667d8c0eb8f, 0x4151f601b1d775f3, 0x4173992c0ec03dcd,]
);
assert_eq!(
obs.sat_velocity_m_s.map(f64::to_bits),
[0xc09c17d81e540ab6, 0x409a192982abbeb7, 0x40926013f2ae8000,]
);
}
#[test]
fn predict_rejects_invalid_entry_inputs() {
let source = static_source([20_200_000.0, 14_000_000.0, 21_700_000.0]);
let sat = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let err = predict(
&source,
sat,
[f64::NAN, 0.0, 0.0],
646_272_000.0,
no_light_time_options(),
)
.expect_err("non-finite receiver position must fail");
assert_invalid_observables_input(
err,
"receiver_ecef_m",
ObservablesInputErrorKind::NonFinite,
);
let err = predict(
&source,
sat,
[0.0, 0.0, 0.0],
f64::INFINITY,
no_light_time_options(),
)
.expect_err("non-finite receive time must fail");
assert_invalid_observables_input(err, "t_rx_j2000_s", ObservablesInputErrorKind::NonFinite);
let mut options = no_light_time_options();
options.carrier_hz = 0.0;
let err = predict(&source, sat, [0.0, 0.0, 0.0], 646_272_000.0, options)
.expect_err("non-positive carrier must fail");
assert_invalid_observables_input(
err,
"options.carrier_hz",
ObservablesInputErrorKind::NotPositive,
);
}
#[test]
fn predict_rejects_invalid_source_state_and_zero_range() {
let sat = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let source = static_source([f64::NAN, 14_000_000.0, 21_700_000.0]);
let err = predict(
&source,
sat,
[0.0, 0.0, 0.0],
646_272_000.0,
no_light_time_options(),
)
.expect_err("non-finite ephemeris position must fail");
assert_invalid_observables_input(
err,
"observable state position_ecef_m",
ObservablesInputErrorKind::NonFinite,
);
let source = static_source([1_000.0, 2_000.0, 3_000.0]);
let err = predict(
&source,
sat,
[1_000.0, 2_000.0, 3_000.0],
646_272_000.0,
no_light_time_options(),
)
.expect_err("zero geometric range must fail");
assert_invalid_observables_input(
err,
"geometric_range_m",
ObservablesInputErrorKind::NotPositive,
);
}
#[test]
fn topocentric_rejects_invalid_receiver_geodetic_conversion() {
let err = topocentric([f64::MAX, 0.0, 0.0], [1.0, 0.0, 0.0], 1.0)
.expect_err("invalid receiver geodetic conversion must fail");
assert_invalid_observables_input(
err,
"receiver_ecef_m",
ObservablesInputErrorKind::OutOfRange,
);
}
const EQUATORIAL_RX_X_M: f64 = 6_378_137.0;
#[test]
fn topocentric_azimuth_is_zero_at_exact_zenith() {
let geometry = topocentric(
[EQUATORIAL_RX_X_M, 0.0, 0.0],
[20_000_000.0, 0.0, 0.0],
20_000_000.0,
)
.expect("zenith topocentric must not error");
assert_eq!(geometry.azimuth_deg, 0.0);
assert!(geometry.azimuth_deg.is_finite());
assert!((geometry.elevation_deg - 90.0).abs() < 1e-9);
}
#[test]
fn topocentric_azimuth_is_zero_just_off_zenith() {
let geometry = topocentric(
[EQUATORIAL_RX_X_M, 0.0, 0.0],
[20_000_000.0, 1.0e-9, 1.0e-9],
20_000_000.0,
)
.expect("near-zenith topocentric must not error");
assert_eq!(geometry.azimuth_deg, 0.0);
assert!(geometry.azimuth_deg.is_finite());
}
#[test]
fn predict_azimuth_is_zero_at_exact_zenith() {
let source = StaticSource {
state: ObservableState {
position_ecef_m: [EQUATORIAL_RX_X_M + 20_000_000.0, 0.0, 0.0],
clock_s: None,
},
};
let sat = GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid satellite id");
let obs = predict(
&source,
sat,
[EQUATORIAL_RX_X_M, 0.0, 0.0],
0.0,
PredictOptions {
carrier_hz: F_L1_HZ,
light_time: false,
sagnac: false,
},
)
.expect("zenith predict must not error");
assert_eq!(obs.azimuth_deg, 0.0);
assert!(obs.azimuth_deg.is_finite());
assert!((obs.elevation_deg - 90.0).abs() < 1e-9);
}
fn batch_test_requests() -> Vec<PredictRequest> {
let sat1 = GnssSatelliteId::new(GnssSystem::Gps, 21).expect("valid satellite id");
let sat2 = GnssSatelliteId::new(GnssSystem::Gps, 7).expect("valid satellite id");
vec![
(sat1, [4_027_894.0, 307_046.0, 4_919_474.0], 646_272_000.0),
(sat2, [4_027_900.0, 307_050.0, 4_919_480.0], 646_272_030.0),
(
sat1,
[1_130_000.0, -4_830_000.0, 3_994_000.0],
646_272_060.0,
),
(
sat2,
[-2_700_000.0, -4_290_000.0, 3_855_000.0],
646_272_090.0,
),
]
}
fn assert_observables_bits_eq(a: &PredictedObservables, b: &PredictedObservables) {
assert_eq!(a.geometric_range_m.to_bits(), b.geometric_range_m.to_bits());
assert_eq!(a.range_rate_m_s.to_bits(), b.range_rate_m_s.to_bits());
assert_eq!(a.doppler_hz.to_bits(), b.doppler_hz.to_bits());
assert_eq!(a.elevation_deg.to_bits(), b.elevation_deg.to_bits());
assert_eq!(a.azimuth_deg.to_bits(), b.azimuth_deg.to_bits());
assert_eq!(a.transmit_offset_us, b.transmit_offset_us);
assert_eq!(
a.transmit_time_j2000_s.to_bits(),
b.transmit_time_j2000_s.to_bits()
);
for k in 0..3 {
assert_eq!(a.los_unit[k].to_bits(), b.los_unit[k].to_bits());
assert_eq!(a.sat_pos_ecef_m[k].to_bits(), b.sat_pos_ecef_m[k].to_bits());
assert_eq!(
a.sat_velocity_m_s[k].to_bits(),
b.sat_velocity_m_s[k].to_bits()
);
}
}
#[test]
fn predict_batch_matches_scalar_loop_bitwise() {
let source = StaticSource {
state: ObservableState {
position_ecef_m: [20_200_000.0, 14_000_000.0, 21_700_000.0],
clock_s: Some(1.25e-6),
},
};
let options = PredictOptions {
carrier_hz: F_L1_HZ,
light_time: true,
sagnac: true,
};
let requests = batch_test_requests();
let batch = predict_batch(&source, &requests, options);
assert_eq!(batch.len(), requests.len());
for (entry, &(sat, rx, t)) in batch.iter().zip(requests.iter()) {
let scalar = predict(&source, sat, rx, t, options);
match (entry, &scalar) {
(Ok(b), Ok(s)) => assert_observables_bits_eq(b, s),
(Err(_), Err(_)) => {}
_ => panic!("batch and scalar predict disagree on Ok/Err"),
}
}
}
#[test]
fn predict_batch_parallel_matches_serial_bitwise() {
let source = StaticSource {
state: ObservableState {
position_ecef_m: [20_200_000.0, 14_000_000.0, 21_700_000.0],
clock_s: Some(1.25e-6),
},
};
let options = PredictOptions {
carrier_hz: F_L1_HZ,
light_time: true,
sagnac: true,
};
let requests = batch_test_requests();
let serial = predict_batch(&source, &requests, options);
let parallel = predict_batch_parallel(&source, &requests, options);
assert_eq!(serial.len(), parallel.len());
for (s, p) in serial.iter().zip(parallel.iter()) {
match (s, p) {
(Ok(a), Ok(b)) => assert_observables_bits_eq(a, b),
(Err(_), Err(_)) => {}
_ => panic!("serial and parallel batch disagree on Ok/Err"),
}
}
}
}