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

sidereon_core/
dgnss.rs
