sidereon_core/
dgnss.rs

1//! Code-differential GNSS (DGPS) pseudorange corrections.
2//!
3//! This module owns the language-independent DGNSS modeling that used to live
4//! in Sidereon: base-station pseudorange correction generation, rover observation
5//! pairing, and the direct corrected-observation SPP orchestration.
6
7use std::collections::BTreeMap;
8
9use crate::astro::math::vec3;
10use crate::constants::C_M_S;
11use crate::id::GnssSatelliteId;
12use crate::observables::{
13    predict, ObservableEphemerisSource, ObservablesError, ObservablesInputErrorKind, PredictOptions,
14};
15use crate::spp::{self, EphemerisSource, Observation, ReceiverSolution, SolveInputs, SppError};
16use crate::validate;
17
18/// A single code pseudorange observation keyed by its RINEX/SP3 satellite token.
19#[derive(Debug, Clone, PartialEq)]
20pub struct CodeObservation {
21    /// Satellite token, e.g. `"G21"`.
22    pub satellite_id: String,
23    /// Measured pseudorange in meters.
24    pub pseudorange_m: f64,
25}
26
27impl CodeObservation {
28    /// Construct a pseudorange observation from a satellite token and range.
29    pub fn new(satellite_id: impl Into<String>, pseudorange_m: f64) -> Self {
30        Self {
31            satellite_id: satellite_id.into(),
32            pseudorange_m,
33        }
34    }
35}
36
37/// Result of applying base corrections to rover observations.
38#[derive(Debug, Clone, PartialEq)]
39pub struct AppliedCorrections {
40    /// Corrected rover pseudoranges, in rover-observation order.
41    pub corrected: Vec<CodeObservation>,
42    /// Rover satellite tokens that had no matching correction, in rover order.
43    pub dropped: Vec<String>,
44}
45
46/// DGNSS rover solve output.
47#[derive(Debug, Clone)]
48pub struct PositionSolution {
49    /// Corrected rover SPP solution.
50    pub solution: ReceiverSolution,
51    /// Rover minus base ECEF vector in meters.
52    pub baseline_vector_m: [f64; 3],
53    /// Baseline length in meters.
54    pub baseline_m: f64,
55    /// Rover satellite tokens without matching base correction.
56    pub dropped_sats: Vec<String>,
57}
58
59/// Error from the DGNSS position orchestration.
60#[derive(Debug, Clone)]
61pub enum DgnssError {
62    /// A public DGNSS input was malformed, non-finite, or outside its physical
63    /// domain.
64    InvalidInput {
65        /// The invalid input field.
66        field: &'static str,
67        /// The validation failure category.
68        reason: &'static str,
69    },
70    /// Corrected-observation SPP solve failed.
71    Spp(SppError),
72}
73
74impl core::fmt::Display for DgnssError {
75    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
76        match self {
77            Self::InvalidInput { field, reason } => {
78                write!(f, "invalid DGNSS input {field}: {reason}")
79            }
80            Self::Spp(err) => write!(f, "{err}"),
81        }
82    }
83}
84
85impl std::error::Error for DgnssError {
86    fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
87        match self {
88            Self::Spp(err) => Some(err),
89            Self::InvalidInput { .. } => None,
90        }
91    }
92}
93
94impl From<SppError> for DgnssError {
95    fn from(value: SppError) -> Self {
96        Self::Spp(value)
97    }
98}
99
100/// Compute per-satellite pseudorange corrections from a surveyed base station.
101///
102/// The correction is `PRC = pr_base - (range_base - c * sat_clock)`, with the
103/// range and satellite clock coming from the same light-time/Sagnac observable
104/// predictor used by the SPP pipeline. Observations with malformed satellite
105/// tokens or unavailable orbit/clock data are skipped, matching Sidereon'
106/// historical "cannot correct this satellite" behavior.
107pub fn pseudorange_corrections(
108    source: &dyn ObservableEphemerisSource,
109    base_position_m: [f64; 3],
110    base_observations: &[CodeObservation],
111    t_rx_j2000_s: f64,
112) -> Result<BTreeMap<String, f64>, DgnssError> {
113    validate_base_position(base_position_m)?;
114    validate::finite(t_rx_j2000_s, "t_rx_j2000_s").map_err(dgnss_invalid_input)?;
115
116    let mut corrections = BTreeMap::new();
117    for obs in base_observations {
118        let pseudorange_m =
119            validate::finite_positive(obs.pseudorange_m, "base_observation.pseudorange_m")
120                .map_err(dgnss_invalid_input)?;
121        let Some(sat) = sat_from_token(&obs.satellite_id) else {
122            continue;
123        };
124        let pred = match predict(
125            source,
126            sat,
127            base_position_m,
128            t_rx_j2000_s,
129            PredictOptions::default(),
130        ) {
131            Ok(pred) => pred,
132            Err(ObservablesError::InvalidInput { field, kind }) => {
133                return Err(invalid_observable_input(field, kind));
134            }
135            Err(_) => continue,
136        };
137        let Some(sat_clock_s) = pred.sat_clock_s else {
138            continue;
139        };
140        let geometric_range_m =
141            validate::finite(pred.geometric_range_m, "predicted.geometric_range_m")
142                .map_err(dgnss_invalid_input)?;
143        let sat_clock_s =
144            validate::finite(sat_clock_s, "predicted.sat_clock_s").map_err(dgnss_invalid_input)?;
145        let modeled_base_m =
146            validate::finite(geometric_range_m - C_M_S * sat_clock_s, "modeled_base_m")
147                .map_err(dgnss_invalid_input)?;
148        let correction_m =
149            validate::finite(pseudorange_m - modeled_base_m, "pseudorange_correction_m")
150                .map_err(dgnss_invalid_input)?;
151        corrections.insert(obs.satellite_id.clone(), correction_m);
152    }
153    Ok(corrections)
154}
155
156/// Apply base pseudorange corrections to rover observations by satellite token.
157///
158/// The output order follows the rover observation order. Corrections without a
159/// rover observation are ignored; rover observations without a correction are
160/// reported in `dropped`.
161pub fn apply_corrections(
162    rover_observations: &[CodeObservation],
163    corrections: &BTreeMap<String, f64>,
164) -> Result<AppliedCorrections, DgnssError> {
165    for prc_m in corrections.values() {
166        validate::finite(*prc_m, "pseudorange_correction_m").map_err(dgnss_invalid_input)?;
167    }
168
169    let mut corrected = Vec::with_capacity(rover_observations.len());
170    let mut dropped = Vec::new();
171    for obs in rover_observations {
172        let pseudorange_m =
173            validate::finite_positive(obs.pseudorange_m, "rover_observation.pseudorange_m")
174                .map_err(dgnss_invalid_input)?;
175        match corrections.get(&obs.satellite_id) {
176            Some(prc_m) => {
177                let corrected_pseudorange_m =
178                    validate::finite_positive(pseudorange_m - prc_m, "corrected_pseudorange_m")
179                        .map_err(dgnss_invalid_input)?;
180                corrected.push(CodeObservation::new(
181                    obs.satellite_id.clone(),
182                    corrected_pseudorange_m,
183                ));
184            }
185            None => dropped.push(obs.satellite_id.clone()),
186        }
187    }
188    Ok(AppliedCorrections { corrected, dropped })
189}
190
191/// Compute DGNSS corrections, apply them to rover observations, and solve SPP.
192///
193/// `solve_inputs` supplies the receive-time scalars, initial guess, meteorology,
194/// Klobuchar coefficients, and optional Huber configuration. Its observations
195/// and atmospheric-correction flags are replaced: DGNSS solves the corrected
196/// rover pseudoranges with ionosphere/troposphere disabled because the
197/// differential already removed common path delays.
198pub fn solve_position<S>(
199    source: &S,
200    base_position_m: [f64; 3],
201    base_observations: &[CodeObservation],
202    rover_observations: &[CodeObservation],
203    mut solve_inputs: SolveInputs,
204    with_geodetic: bool,
205) -> Result<PositionSolution, DgnssError>
206where
207    S: ObservableEphemerisSource + EphemerisSource,
208{
209    let corrections = pseudorange_corrections(
210        source,
211        base_position_m,
212        base_observations,
213        solve_inputs.t_rx_j2000_s,
214    )?;
215    let applied = apply_corrections(rover_observations, &corrections)?;
216    solve_inputs.observations = applied
217        .corrected
218        .iter()
219        .filter_map(|obs| {
220            sat_from_token(&obs.satellite_id).map(|satellite_id| Observation {
221                satellite_id,
222                pseudorange_m: obs.pseudorange_m,
223            })
224        })
225        .collect();
226    solve_inputs.corrections = spp::Corrections::NONE;
227
228    let solution = spp::solve(source, &solve_inputs, with_geodetic)?;
229    let pos = solution.position.as_array();
230    let baseline_vector_m = vec3::sub3(pos, base_position_m);
231    let baseline_m = vec3::norm3(baseline_vector_m);
232
233    Ok(PositionSolution {
234        solution,
235        baseline_vector_m,
236        baseline_m,
237        dropped_sats: applied.dropped,
238    })
239}
240
241fn sat_from_token(token: &str) -> Option<GnssSatelliteId> {
242    token.parse::<GnssSatelliteId>().ok()
243}
244
245fn validate_base_position(base_position_m: [f64; 3]) -> Result<(), DgnssError> {
246    const FIELDS: [&str; 3] = [
247        "base_position_m[0]",
248        "base_position_m[1]",
249        "base_position_m[2]",
250    ];
251    for (value, field) in base_position_m.into_iter().zip(FIELDS) {
252        validate::finite(value, field).map_err(dgnss_invalid_input)?;
253    }
254    Ok(())
255}
256
257fn dgnss_invalid_input(error: validate::FieldError) -> DgnssError {
258    DgnssError::InvalidInput {
259        field: error.field(),
260        reason: error.reason(),
261    }
262}
263
264fn invalid_observable_input(field: &'static str, kind: ObservablesInputErrorKind) -> DgnssError {
265    DgnssError::InvalidInput {
266        field,
267        reason: observable_input_reason(kind),
268    }
269}
270
271fn observable_input_reason(kind: ObservablesInputErrorKind) -> &'static str {
272    match kind {
273        ObservablesInputErrorKind::NonFinite => "not finite",
274        ObservablesInputErrorKind::NotPositive => "not positive",
275        ObservablesInputErrorKind::Negative => "negative",
276        ObservablesInputErrorKind::OutOfRange => "out of range",
277        ObservablesInputErrorKind::Missing => "missing",
278        ObservablesInputErrorKind::FloatParse => "invalid float",
279        ObservablesInputErrorKind::IntParse => "invalid integer",
280        ObservablesInputErrorKind::InvalidCivilDate => "invalid civil date",
281        ObservablesInputErrorKind::InvalidCivilTime => "invalid civil time",
282    }
283}
sidereon_core/dgnss.rs

sidereon_core/
dgnss.rs
