sidereon_core/
static_reference_station.rs

1//! Static reference-station solve from paired rover/reference RINEX arcs.
2//!
3//! This module composes existing language-independent pieces: RINEX SPP epoch
4//! assembly, code-DGNSS pseudorange corrections, stacked static SPP, and the RTK
5//! RINEX arc builders/static carrier solver. It returns a station coordinate
6//! with a covariance from the final normal equations, not from epoch scatter.
7
8use std::collections::{BTreeMap, BTreeSet};
9
10use crate::astro::math::vec3;
11use crate::dgnss::{apply_corrections, pseudorange_corrections, solve_position, CodeObservation};
12use crate::dop::rotate_covariance_ecef_to_enu_m2;
13use crate::frame::{itrf_to_geodetic, ItrfPositionM, Wgs84Geodetic};
14use crate::observables::ObservableEphemerisSource;
15use crate::positioning::{
16    spp_inputs_from_rinex_obs, EphemerisSource, RinexSppAssemblySource, RinexSppEpochInputs,
17    RinexSppOptions, StaticEpoch, StaticSolution, StaticSolveOptions,
18};
19use crate::rinex::observations::{ObsEpochTime, RinexObs};
20use crate::rtk_filter::{
21    build_rinex_rtk_arc, IntegerStatus, RtkRinexArcOptions, RtkStaticArcConfig,
22    RtkStaticArcSolution,
23};
24use crate::spp::{Corrections, Observation};
25use crate::validate;
26
27/// High-level solve mode selected for the reported station coordinate.
28#[derive(Debug, Clone, Copy, PartialEq, Eq)]
29pub enum StaticReferenceStationMode {
30    /// Code-DGNSS corrected pseudoranges stacked through the static SPP solver.
31    CodeDgnss,
32    /// Carrier RTK float baseline added to the surveyed reference coordinate.
33    CarrierFloat,
34    /// Carrier RTK integer-fixed baseline added to the surveyed reference coordinate.
35    CarrierFixed,
36}
37
38/// Fix label for the reported station coordinate.
39#[derive(Debug, Clone, Copy, PartialEq, Eq)]
40pub enum StaticReferenceFixStatus {
41    /// Code-DGNSS solution; no carrier integer fix was attempted for the selected result.
42    CodeDgnss,
43    /// Carrier RTK float baseline.
44    CarrierFloat,
45    /// Carrier RTK integer-fixed baseline.
46    CarrierFixed,
47}
48
49impl From<StaticReferenceFixStatus> for crate::fusion::GnssFixStatus {
50    fn from(value: StaticReferenceFixStatus) -> Self {
51        match value {
52            StaticReferenceFixStatus::CodeDgnss => Self::Single,
53            StaticReferenceFixStatus::CarrierFloat => Self::Float,
54            StaticReferenceFixStatus::CarrierFixed => Self::Fixed,
55        }
56    }
57}
58
59/// Attempt status for one mode.
60#[derive(Debug, Clone, Copy, PartialEq, Eq)]
61pub enum StaticReferenceModeStatus {
62    /// The mode was enabled and solved.
63    Solved,
64    /// The mode was enabled but failed.
65    Failed,
66}
67
68/// Position covariance for the returned station coordinate.
69#[derive(Debug, Clone, Copy, PartialEq)]
70pub struct StaticReferenceStationCovariance {
71    /// ECEF position covariance in square metres.
72    pub position_ecef_m2: [[f64; 3]; 3],
73    /// Local ENU position covariance in square metres.
74    pub position_enu_m2: [[f64; 3]; 3],
75}
76
77/// Per-epoch diagnostic rollup for a solved mode.
78#[derive(Debug, Clone, PartialEq)]
79pub struct StaticReferenceEpochDiagnostic {
80    /// Mode that produced this diagnostic row.
81    pub mode: StaticReferenceStationMode,
82    /// Epoch index in the assembled solve input for this mode.
83    pub epoch_index: usize,
84    /// Satellites used by the epoch, sorted by the underlying solver.
85    pub used_satellites: Vec<String>,
86    /// Number of rejected satellites for code-DGNSS epochs.
87    pub rejected_satellite_count: usize,
88    /// Code residual RMS for the epoch, metres, when code rows are available.
89    pub code_residual_rms_m: Option<f64>,
90    /// Carrier residual RMS for the epoch, metres, when carrier rows are available.
91    pub phase_residual_rms_m: Option<f64>,
92    /// Total unweighted residual RMS for the epoch, metres.
93    pub residual_rms_m: Option<f64>,
94}
95
96/// Report for a mode attempted by the wrapper.
97#[derive(Debug, Clone, PartialEq)]
98pub struct StaticReferenceModeReport {
99    /// Mode attempted.
100    pub mode: StaticReferenceStationMode,
101    /// Whether the mode solved or failed.
102    pub status: StaticReferenceModeStatus,
103    /// Solved epoch count or zero on failure.
104    pub used_epochs: usize,
105    /// Number of raw RINEX epochs skipped by the mode builder.
106    pub skipped_epochs: usize,
107    /// Measurement rows used by the final solve, when known.
108    pub used_measurements: usize,
109    /// Failure detail, when the mode failed.
110    pub error: Option<StaticReferenceModeError>,
111}
112
113/// Typed failure detail for one attempted static reference-station mode.
114#[derive(Debug, Clone, PartialEq)]
115pub enum StaticReferenceModeError {
116    /// RINEX/SPP input assembly failed before mode-specific solving.
117    RinexAssembly {
118        /// Observation side being assembled.
119        side: &'static str,
120        /// Source error text.
121        reason: String,
122    },
123    /// Code-DGNSS reference and rover assemblies had no epoch in common.
124    NoMatchedCodeEpochs,
125    /// Code-DGNSS correction or single-epoch solve failed.
126    CodeDgnss {
127        /// Source error text.
128        reason: String,
129    },
130    /// Multi-epoch static code solve failed.
131    StaticSolve {
132        /// Source error text.
133        reason: String,
134    },
135    /// Carrier RTK arc construction failed.
136    CarrierArc {
137        /// Source error text.
138        reason: String,
139    },
140    /// Carrier static RTK solve failed.
141    CarrierSolve {
142        /// Source error text.
143        reason: String,
144    },
145    /// Frame, coordinate, or covariance conversion failed.
146    Frame {
147        /// Conversion field.
148        field: &'static str,
149        /// Source error text.
150        reason: String,
151    },
152    /// Corrected code observations could not be applied or converted.
153    CorrectedObservation {
154        /// Source error text.
155        reason: String,
156    },
157    /// A corrected satellite identifier could not be parsed back to a typed ID.
158    InvalidCorrectedSatelliteId {
159        /// Invalid satellite identifier text.
160        satellite_id: String,
161    },
162}
163
164impl core::fmt::Display for StaticReferenceModeError {
165    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
166        match self {
167            Self::RinexAssembly { side, reason } => {
168                write!(f, "{side} RINEX assembly failed: {reason}")
169            }
170            Self::NoMatchedCodeEpochs => f.write_str("no matched epochs"),
171            Self::CodeDgnss { reason } => {
172                write!(f, "code-DGNSS failed: {reason}")
173            }
174            Self::StaticSolve { reason } => {
175                write!(f, "static code solve failed: {reason}")
176            }
177            Self::CarrierArc { reason } => {
178                write!(f, "carrier RTK arc failed: {reason}")
179            }
180            Self::CarrierSolve { reason } => {
181                write!(f, "carrier RTK solve failed: {reason}")
182            }
183            Self::Frame { field, reason } => {
184                write!(f, "{field} conversion failed: {reason}")
185            }
186            Self::CorrectedObservation { reason } => {
187                write!(f, "corrected observation failed: {reason}")
188            }
189            Self::InvalidCorrectedSatelliteId { satellite_id } => {
190                write!(f, "invalid corrected satellite id {satellite_id}")
191            }
192        }
193    }
194}
195
196impl std::error::Error for StaticReferenceModeError {}
197
198/// Code-DGNSS static solve detail.
199#[derive(Debug, Clone, PartialEq)]
200pub struct StaticReferenceCodeSolution {
201    /// Solved rover/reference-station coordinate.
202    pub position: ItrfPositionM,
203    /// Geodetic coordinate when requested.
204    pub geodetic: Option<Wgs84Geodetic>,
205    /// Position covariance from the code-DGNSS normal equations.
206    pub covariance: StaticReferenceStationCovariance,
207    /// Multi-epoch static solution when the code path stacked more than one epoch.
208    pub static_solution: Option<StaticSolution>,
209    /// Baseline vector, rover minus reference, metres.
210    pub baseline_vector_m: [f64; 3],
211    /// Baseline length, metres.
212    pub baseline_m: f64,
213    /// Per-epoch diagnostic rollups.
214    pub diagnostics: Vec<StaticReferenceEpochDiagnostic>,
215}
216
217/// Carrier RTK static solve detail.
218#[derive(Debug, Clone, PartialEq)]
219pub struct StaticReferenceCarrierSolution {
220    /// Solved rover/reference-station coordinate.
221    pub position: ItrfPositionM,
222    /// Geodetic coordinate when requested.
223    pub geodetic: Option<Wgs84Geodetic>,
224    /// Position covariance from the selected carrier baseline normal equations.
225    pub covariance: StaticReferenceStationCovariance,
226    /// Selected baseline vector, rover minus reference, metres.
227    pub baseline_vector_m: [f64; 3],
228    /// Baseline length, metres.
229    pub baseline_m: f64,
230    /// Integer ambiguity status from the fixed RTK solve.
231    pub integer_status: IntegerStatus,
232    /// Integer ratio from the fixed RTK solve, when a search ran.
233    pub integer_ratio: Option<f64>,
234    /// Full carrier static arc solution.
235    pub rtk_solution: RtkStaticArcSolution,
236    /// Per-epoch diagnostic rollups from the selected float/fixed residuals.
237    pub diagnostics: Vec<StaticReferenceEpochDiagnostic>,
238}
239
240/// Final station solution returned by the RINEX wrapper.
241#[derive(Debug, Clone, PartialEq)]
242pub struct StaticReferenceStationSolution {
243    /// Selected mode used for the reported coordinate.
244    pub mode: StaticReferenceStationMode,
245    /// Reported fix status for the selected coordinate.
246    pub fix_status: StaticReferenceFixStatus,
247    /// Solved rover/reference-station coordinate.
248    pub position: ItrfPositionM,
249    /// Geodetic coordinate when requested.
250    pub geodetic: Option<Wgs84Geodetic>,
251    /// Position covariance for the reported coordinate.
252    pub covariance: StaticReferenceStationCovariance,
253    /// Baseline vector, rover minus reference, metres.
254    pub baseline_vector_m: [f64; 3],
255    /// Baseline length, metres.
256    pub baseline_m: f64,
257    /// Code-DGNSS solution detail when the mode was enabled and solved.
258    pub code_solution: Option<StaticReferenceCodeSolution>,
259    /// Carrier RTK solution detail when the mode was enabled and solved.
260    pub carrier_solution: Option<StaticReferenceCarrierSolution>,
261    /// Per-mode attempt reports.
262    pub mode_reports: Vec<StaticReferenceModeReport>,
263    /// Diagnostics for the selected mode.
264    pub diagnostics: Vec<StaticReferenceEpochDiagnostic>,
265}
266
267/// Carrier RTK options for the RINEX station wrapper.
268#[derive(Debug, Clone, PartialEq)]
269pub struct StaticReferenceCarrierRinexOptions {
270    /// RINEX-to-RTK arc extraction options.
271    pub arc_options: RtkRinexArcOptions,
272    /// Static RTK solve configuration. The wrapper overwrites its base position
273    /// and ambiguity scale maps from the function inputs and RINEX arc.
274    pub static_config: RtkStaticArcConfig,
275}
276
277/// RINEX wrapper options. `None` disables that mode.
278#[derive(Debug, Clone, PartialEq)]
279pub struct StaticReferenceStationRinexOptions {
280    /// Code-DGNSS RINEX/SPP assembly options.
281    pub code_options: Option<RinexSppOptions>,
282    /// Carrier RTK RINEX/static options.
283    pub carrier_options: Option<StaticReferenceCarrierRinexOptions>,
284    /// Whether to include geodetic coordinates in the selected and nested results.
285    pub with_geodetic: bool,
286}
287
288impl StaticReferenceStationRinexOptions {
289    /// Build options with both code-DGNSS and carrier RTK enabled.
290    #[must_use]
291    pub const fn code_and_carrier(
292        code_options: RinexSppOptions,
293        carrier_options: StaticReferenceCarrierRinexOptions,
294        with_geodetic: bool,
295    ) -> Self {
296        Self {
297            code_options: Some(code_options),
298            carrier_options: Some(carrier_options),
299            with_geodetic,
300        }
301    }
302
303    /// Build options with only code-DGNSS enabled.
304    #[must_use]
305    pub const fn code_only(code_options: RinexSppOptions, with_geodetic: bool) -> Self {
306        Self {
307            code_options: Some(code_options),
308            carrier_options: None,
309            with_geodetic,
310        }
311    }
312
313    /// Build options with only carrier RTK enabled.
314    #[must_use]
315    pub const fn carrier_only(
316        carrier_options: StaticReferenceCarrierRinexOptions,
317        with_geodetic: bool,
318    ) -> Self {
319        Self {
320            code_options: None,
321            carrier_options: Some(carrier_options),
322            with_geodetic,
323        }
324    }
325}
326
327/// Error returned by [`solve_static_reference_station_rinex`].
328#[derive(Debug, Clone, PartialEq)]
329pub enum StaticReferenceStationError {
330    /// Public input validation failed.
331    InvalidInput {
332        /// Invalid input field.
333        field: &'static str,
334        /// Validation reason.
335        reason: &'static str,
336    },
337    /// Neither code-DGNSS nor carrier RTK mode was enabled.
338    NoEnabledModes,
339    /// Every enabled mode failed. Individual errors are carried in the reports.
340    AllModesFailed {
341        /// Reports for the failed modes.
342        mode_reports: Vec<StaticReferenceModeReport>,
343    },
344}
345
346impl core::fmt::Display for StaticReferenceStationError {
347    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
348        match self {
349            Self::InvalidInput { field, reason } => {
350                write!(
351                    f,
352                    "invalid static reference-station input {field}: {reason}"
353                )
354            }
355            Self::NoEnabledModes => {
356                f.write_str("static reference-station solve has no enabled modes")
357            }
358            Self::AllModesFailed { mode_reports } => {
359                f.write_str("all static reference-station modes failed")?;
360                for report in mode_reports {
361                    if let Some(error) = &report.error {
362                        write!(f, "; {}: {error}", mode_label(report.mode))?;
363                    } else {
364                        write!(f, "; {}: failed", mode_label(report.mode))?;
365                    }
366                }
367                Ok(())
368            }
369        }
370    }
371}
372
373impl std::error::Error for StaticReferenceStationError {}
374
375/// Solve one rover/reference-station coordinate from paired reference and rover
376/// RINEX observation files plus a known reference coordinate.
377///
378/// `reference_position_m` is the ECEF coordinate of the point represented by the
379/// reference observations. The returned coordinate is the corresponding point
380/// represented by the rover observations; caller-owned antenna marker/ARP
381/// conversions should be applied consistently before and after this call.
382pub fn solve_static_reference_station_rinex<S>(
383    source: &S,
384    reference_obs: &RinexObs,
385    rover_obs: &RinexObs,
386    reference_position_m: [f64; 3],
387    options: &StaticReferenceStationRinexOptions,
388) -> Result<StaticReferenceStationSolution, StaticReferenceStationError>
389where
390    S: EphemerisSource + ObservableEphemerisSource + RinexSppAssemblySource,
391{
392    validate::finite_vec3(reference_position_m, "reference_position_m")
393        .map_err(static_reference_input_error)?;
394
395    if options.code_options.is_none() && options.carrier_options.is_none() {
396        return Err(StaticReferenceStationError::NoEnabledModes);
397    }
398
399    let mut reports = Vec::new();
400    let code = match &options.code_options {
401        Some(code_options) => match solve_code_dgnss_static(
402            source,
403            reference_obs,
404            rover_obs,
405            reference_position_m,
406            code_options,
407            options.with_geodetic,
408        ) {
409            Ok(solution) => {
410                reports.push(StaticReferenceModeReport {
411                    mode: StaticReferenceStationMode::CodeDgnss,
412                    status: StaticReferenceModeStatus::Solved,
413                    used_epochs: solution.diagnostics.len(),
414                    skipped_epochs: 0,
415                    used_measurements: solution
416                        .diagnostics
417                        .iter()
418                        .map(|row| row.used_satellites.len())
419                        .sum(),
420                    error: None,
421                });
422                Some(solution)
423            }
424            Err(error) => {
425                reports.push(failed_report(StaticReferenceStationMode::CodeDgnss, error));
426                None
427            }
428        },
429        None => None,
430    };
431
432    let carrier = match &options.carrier_options {
433        Some(carrier_options) => match solve_carrier_static(
434            source,
435            reference_obs,
436            rover_obs,
437            reference_position_m,
438            carrier_options,
439            options.with_geodetic,
440        ) {
441            Ok((solution, skipped_epochs)) => {
442                reports.push(StaticReferenceModeReport {
443                    mode: solution_mode_from_carrier(&solution),
444                    status: StaticReferenceModeStatus::Solved,
445                    used_epochs: solution.diagnostics.len(),
446                    skipped_epochs,
447                    used_measurements: carrier_used_measurements(&solution),
448                    error: None,
449                });
450                Some(solution)
451            }
452            Err(error) => {
453                reports.push(failed_report(
454                    StaticReferenceStationMode::CarrierFixed,
455                    error,
456                ));
457                None
458            }
459        },
460        None => None,
461    };
462
463    let selected = select_solution(reference_position_m, code, carrier, reports)?;
464    Ok(selected)
465}
466
467fn solve_code_dgnss_static<S>(
468    source: &S,
469    reference_obs: &RinexObs,
470    rover_obs: &RinexObs,
471    reference_position_m: [f64; 3],
472    code_options: &RinexSppOptions,
473    with_geodetic: bool,
474) -> Result<StaticReferenceCodeSolution, StaticReferenceModeError>
475where
476    S: EphemerisSource + ObservableEphemerisSource + RinexSppAssemblySource,
477{
478    let reference_epochs =
479        spp_inputs_from_rinex_obs(reference_obs, source, code_options).map_err(|error| {
480            StaticReferenceModeError::RinexAssembly {
481                side: "reference",
482                reason: error.to_string(),
483            }
484        })?;
485    let rover_epochs =
486        spp_inputs_from_rinex_obs(rover_obs, source, code_options).map_err(|error| {
487            StaticReferenceModeError::RinexAssembly {
488                side: "rover",
489                reason: error.to_string(),
490            }
491        })?;
492    let matched = matched_code_epochs(&reference_epochs, &rover_epochs);
493    if matched.is_empty() {
494        return Err(StaticReferenceModeError::NoMatchedCodeEpochs);
495    }
496
497    if matched.len() == 1 {
498        return solve_single_code_epoch(source, reference_position_m, matched[0], with_geodetic);
499    }
500
501    let static_options = StaticSolveOptions::from_solve_inputs(&matched[0].1.inputs, with_geodetic);
502    let mut static_epochs = Vec::with_capacity(matched.len());
503    for (reference_epoch, rover_epoch) in matched {
504        let corrected = corrected_rover_observations(
505            source,
506            reference_position_m,
507            reference_epoch,
508            rover_epoch,
509        )?;
510        let mut inputs = rover_epoch.inputs.clone();
511        inputs.observations = corrected;
512        inputs.corrections = Corrections::NONE;
513        let mut static_epoch = StaticEpoch::from_solve_inputs(inputs);
514        static_epoch.weights = Some(vec![0.5; static_epoch.measurements.len()]);
515        static_epochs.push(static_epoch);
516    }
517
518    let static_solution = crate::static_positioning::solve_static_without_influence(
519        source,
520        &static_epochs,
521        static_options,
522    )
523    .map_err(|error| StaticReferenceModeError::StaticSolve {
524        reason: error.to_string(),
525    })?;
526    let position = static_solution.position;
527    let covariance = StaticReferenceStationCovariance {
528        position_ecef_m2: static_solution.covariance.position_ecef_m2,
529        position_enu_m2: static_solution.covariance.position_enu_m2,
530    };
531    let baseline_vector_m = vec3::sub3(position.as_array(), reference_position_m);
532    let baseline_m = vec3::norm3(baseline_vector_m);
533    let diagnostics = code_static_diagnostics(&static_solution);
534
535    Ok(StaticReferenceCodeSolution {
536        position,
537        geodetic: static_solution.geodetic,
538        covariance,
539        static_solution: Some(static_solution),
540        baseline_vector_m,
541        baseline_m,
542        diagnostics,
543    })
544}
545
546fn solve_single_code_epoch<S>(
547    source: &S,
548    reference_position_m: [f64; 3],
549    matched: (&RinexSppEpochInputs, &RinexSppEpochInputs),
550    with_geodetic: bool,
551) -> Result<StaticReferenceCodeSolution, StaticReferenceModeError>
552where
553    S: EphemerisSource + ObservableEphemerisSource,
554{
555    let (reference_epoch, rover_epoch) = matched;
556    let reference_codes = code_observations(&reference_epoch.inputs.observations);
557    let rover_codes = code_observations(&rover_epoch.inputs.observations);
558    let solution = solve_position(
559        source,
560        reference_position_m,
561        &reference_codes,
562        &rover_codes,
563        rover_epoch.inputs.clone(),
564        with_geodetic,
565    )
566    .map_err(|error| StaticReferenceModeError::CodeDgnss {
567        reason: error.to_string(),
568    })?;
569    let covariance = StaticReferenceStationCovariance {
570        position_ecef_m2: solution.solution.position_covariance.ecef_m2,
571        position_enu_m2: solution.solution.position_covariance.enu_m2,
572    };
573    let position = solution.solution.position;
574    let diagnostics = vec![StaticReferenceEpochDiagnostic {
575        mode: StaticReferenceStationMode::CodeDgnss,
576        epoch_index: 0,
577        used_satellites: solution
578            .solution
579            .used_sats
580            .iter()
581            .map(ToString::to_string)
582            .collect(),
583        rejected_satellite_count: solution.solution.rejected_sats.len(),
584        code_residual_rms_m: Some(solution.solution.residual_rms_m()),
585        phase_residual_rms_m: None,
586        residual_rms_m: Some(solution.solution.residual_rms_m()),
587    }];
588
589    Ok(StaticReferenceCodeSolution {
590        position,
591        geodetic: solution.solution.geodetic,
592        covariance,
593        static_solution: None,
594        baseline_vector_m: solution.baseline_vector_m,
595        baseline_m: solution.baseline_m,
596        diagnostics,
597    })
598}
599
600fn solve_carrier_static<S>(
601    source: &S,
602    reference_obs: &RinexObs,
603    rover_obs: &RinexObs,
604    reference_position_m: [f64; 3],
605    carrier_options: &StaticReferenceCarrierRinexOptions,
606    with_geodetic: bool,
607) -> Result<(StaticReferenceCarrierSolution, usize), StaticReferenceModeError>
608where
609    S: ObservableEphemerisSource,
610{
611    let arc = build_rinex_rtk_arc(
612        source,
613        reference_obs,
614        rover_obs,
615        &carrier_options.arc_options,
616    )
617    .map_err(|error| StaticReferenceModeError::CarrierArc {
618        reason: error.to_string(),
619    })?;
620    let mut config = carrier_options.static_config.clone();
621    config.arc.base_m = reference_position_m;
622    config.arc.wavelengths_m = arc.wavelengths_m.clone();
623    config.arc.offsets_m = arc.offsets_m.clone();
624    let rtk_solution =
625        crate::rtk_filter::solve_static_rtk_arc(&arc.epochs, &config).map_err(|error| {
626            StaticReferenceModeError::CarrierSolve {
627                reason: error.to_string(),
628            }
629        })?;
630    let fixed = &rtk_solution.fixed_solution.fixed_solution;
631    let (baseline_vector_m, covariance_ecef_m2, diagnostics) =
632        if fixed.search.integer_status == IntegerStatus::Fixed {
633            (
634                fixed.baseline_m,
635                fixed.baseline_covariance_m2,
636                carrier_diagnostics(StaticReferenceStationMode::CarrierFixed, &fixed.residuals),
637            )
638        } else {
639            (
640                rtk_solution.float_solution.baseline_m,
641                rtk_solution.float_solution.baseline_covariance_m2,
642                carrier_diagnostics(
643                    StaticReferenceStationMode::CarrierFloat,
644                    &rtk_solution.float_solution.residuals,
645                ),
646            )
647        };
648    let position_m = vec3::add3(reference_position_m, baseline_vector_m);
649    let position =
650        ItrfPositionM::new(position_m[0], position_m[1], position_m[2]).map_err(|error| {
651            StaticReferenceModeError::Frame {
652                field: "position",
653                reason: error.to_string(),
654            }
655        })?;
656    let covariance = covariance_from_ecef(position, covariance_ecef_m2)?;
657    let geodetic = if with_geodetic {
658        Some(
659            itrf_to_geodetic(position).map_err(|error| StaticReferenceModeError::Frame {
660                field: "geodetic",
661                reason: error.to_string(),
662            })?,
663        )
664    } else {
665        None
666    };
667
668    Ok((
669        StaticReferenceCarrierSolution {
670            position,
671            geodetic,
672            covariance,
673            baseline_vector_m,
674            baseline_m: vec3::norm3(baseline_vector_m),
675            integer_status: fixed.search.integer_status,
676            integer_ratio: fixed.search.integer_ratio,
677            rtk_solution,
678            diagnostics,
679        },
680        arc.skipped_epoch_count,
681    ))
682}
683
684fn select_solution(
685    reference_position_m: [f64; 3],
686    code: Option<StaticReferenceCodeSolution>,
687    carrier: Option<StaticReferenceCarrierSolution>,
688    reports: Vec<StaticReferenceModeReport>,
689) -> Result<StaticReferenceStationSolution, StaticReferenceStationError> {
690    if code.is_none() && carrier.is_none() {
691        return Err(StaticReferenceStationError::AllModesFailed {
692            mode_reports: reports,
693        });
694    }
695
696    let (mode, position, geodetic, covariance, baseline_vector_m, mut baseline_m, diagnostics) =
697        match (carrier.as_ref(), code.as_ref()) {
698            (Some(carrier), _)
699                if solution_mode_from_carrier(carrier)
700                    == StaticReferenceStationMode::CarrierFixed =>
701            {
702                (
703                    StaticReferenceStationMode::CarrierFixed,
704                    carrier.position,
705                    carrier.geodetic,
706                    carrier.covariance,
707                    carrier.baseline_vector_m,
708                    carrier.baseline_m,
709                    carrier.diagnostics.clone(),
710                )
711            }
712            (_, Some(code)) => (
713                StaticReferenceStationMode::CodeDgnss,
714                code.position,
715                code.geodetic,
716                code.covariance,
717                code.baseline_vector_m,
718                code.baseline_m,
719                code.diagnostics.clone(),
720            ),
721            (Some(carrier), None) => (
722                StaticReferenceStationMode::CarrierFloat,
723                carrier.position,
724                carrier.geodetic,
725                carrier.covariance,
726                carrier.baseline_vector_m,
727                carrier.baseline_m,
728                carrier.diagnostics.clone(),
729            ),
730            (None, None) => unreachable!("handled above"),
731        };
732    let baseline_vector_m = if baseline_vector_m.iter().all(|value| value.is_finite()) {
733        baseline_vector_m
734    } else {
735        let fallback = vec3::sub3(position.as_array(), reference_position_m);
736        baseline_m = vec3::norm3(fallback);
737        fallback
738    };
739
740    Ok(StaticReferenceStationSolution {
741        mode,
742        fix_status: fix_status_from_mode(mode),
743        position,
744        geodetic,
745        covariance,
746        baseline_vector_m,
747        baseline_m,
748        code_solution: code,
749        carrier_solution: carrier,
750        mode_reports: reports,
751        diagnostics,
752    })
753}
754
755fn matched_code_epochs<'a>(
756    reference_epochs: &'a [RinexSppEpochInputs],
757    rover_epochs: &'a [RinexSppEpochInputs],
758) -> Vec<(&'a RinexSppEpochInputs, &'a RinexSppEpochInputs)> {
759    let rover_by_epoch = rover_epochs
760        .iter()
761        .map(|epoch| (epoch_key(epoch.epoch), epoch))
762        .collect::<BTreeMap<_, _>>();
763    reference_epochs
764        .iter()
765        .filter_map(|reference_epoch| {
766            rover_by_epoch
767                .get(&epoch_key(reference_epoch.epoch))
768                .map(|&rover_epoch| (reference_epoch, rover_epoch))
769        })
770        .collect()
771}
772
773fn corrected_rover_observations<S>(
774    source: &S,
775    reference_position_m: [f64; 3],
776    reference_epoch: &RinexSppEpochInputs,
777    rover_epoch: &RinexSppEpochInputs,
778) -> Result<Vec<Observation>, StaticReferenceModeError>
779where
780    S: ObservableEphemerisSource,
781{
782    let corrections = pseudorange_corrections(
783        source,
784        reference_position_m,
785        &code_observations(&reference_epoch.inputs.observations),
786        reference_epoch.inputs.t_rx_j2000_s,
787    )
788    .map_err(|error| StaticReferenceModeError::CodeDgnss {
789        reason: error.to_string(),
790    })?;
791    let corrected = apply_corrections(
792        &code_observations(&rover_epoch.inputs.observations),
793        &corrections,
794    )
795    .map_err(|error| StaticReferenceModeError::CorrectedObservation {
796        reason: error.to_string(),
797    })?;
798    corrected
799        .corrected
800        .into_iter()
801        .map(|obs| {
802            obs.satellite_id
803                .parse()
804                .map(|satellite_id| Observation {
805                    satellite_id,
806                    pseudorange_m: obs.pseudorange_m,
807                })
808                .map_err(|_| StaticReferenceModeError::InvalidCorrectedSatelliteId {
809                    satellite_id: obs.satellite_id,
810                })
811        })
812        .collect()
813}
814
815fn code_observations(observations: &[Observation]) -> Vec<CodeObservation> {
816    observations
817        .iter()
818        .map(|obs| CodeObservation::new(obs.satellite_id.to_string(), obs.pseudorange_m))
819        .collect()
820}
821
822fn code_static_diagnostics(solution: &StaticSolution) -> Vec<StaticReferenceEpochDiagnostic> {
823    let mut residuals_by_epoch = BTreeMap::<usize, Vec<f64>>::new();
824    for residual in &solution.residuals_m {
825        residuals_by_epoch
826            .entry(residual.epoch_index)
827            .or_default()
828            .push(residual.residual_m);
829    }
830
831    solution
832        .used_sats
833        .iter()
834        .enumerate()
835        .map(|(epoch_index, sats)| {
836            let residuals = residuals_by_epoch
837                .get(&epoch_index)
838                .map(Vec::as_slice)
839                .unwrap_or(&[]);
840            let rms = residuals_rms(residuals.iter().copied());
841            StaticReferenceEpochDiagnostic {
842                mode: StaticReferenceStationMode::CodeDgnss,
843                epoch_index,
844                used_satellites: sats.iter().map(ToString::to_string).collect(),
845                rejected_satellite_count: solution
846                    .rejected_sats
847                    .get(epoch_index)
848                    .map_or(0, Vec::len),
849                code_residual_rms_m: rms,
850                phase_residual_rms_m: None,
851                residual_rms_m: rms,
852            }
853        })
854        .collect()
855}
856
857fn carrier_diagnostics(
858    mode: StaticReferenceStationMode,
859    residuals: &[crate::rtk_filter::FloatResidual],
860) -> Vec<StaticReferenceEpochDiagnostic> {
861    #[derive(Default)]
862    struct Accum {
863        sats: BTreeSet<String>,
864        code: Vec<f64>,
865        phase: Vec<f64>,
866    }
867
868    let mut by_epoch = BTreeMap::<usize, Accum>::new();
869    for residual in residuals {
870        let entry = by_epoch.entry(residual.epoch_index).or_default();
871        entry.sats.insert(residual.satellite_id.clone());
872        entry.code.push(residual.code_m);
873        entry.phase.push(residual.phase_m);
874    }
875
876    by_epoch
877        .into_iter()
878        .map(|(epoch_index, accum)| {
879            let total = accum
880                .code
881                .iter()
882                .chain(accum.phase.iter())
883                .copied()
884                .collect::<Vec<_>>();
885            StaticReferenceEpochDiagnostic {
886                mode,
887                epoch_index,
888                used_satellites: accum.sats.into_iter().collect(),
889                rejected_satellite_count: 0,
890                code_residual_rms_m: residuals_rms(accum.code),
891                phase_residual_rms_m: residuals_rms(accum.phase),
892                residual_rms_m: residuals_rms(total),
893            }
894        })
895        .collect()
896}
897
898fn covariance_from_ecef(
899    position: ItrfPositionM,
900    position_ecef_m2: [[f64; 3]; 3],
901) -> Result<StaticReferenceStationCovariance, StaticReferenceModeError> {
902    let geodetic = itrf_to_geodetic(position).map_err(|error| StaticReferenceModeError::Frame {
903        field: "geodetic",
904        reason: error.to_string(),
905    })?;
906    let position_enu_m2 =
907        rotate_covariance_ecef_to_enu_m2(position_ecef_m2, geodetic).map_err(|error| {
908            StaticReferenceModeError::Frame {
909                field: "covariance",
910                reason: error.to_string(),
911            }
912        })?;
913    Ok(StaticReferenceStationCovariance {
914        position_ecef_m2,
915        position_enu_m2,
916    })
917}
918
919fn solution_mode_from_carrier(
920    solution: &StaticReferenceCarrierSolution,
921) -> StaticReferenceStationMode {
922    if solution.integer_status == IntegerStatus::Fixed {
923        StaticReferenceStationMode::CarrierFixed
924    } else {
925        StaticReferenceStationMode::CarrierFloat
926    }
927}
928
929fn carrier_used_measurements(solution: &StaticReferenceCarrierSolution) -> usize {
930    if solution.integer_status == IntegerStatus::Fixed {
931        solution
932            .rtk_solution
933            .fixed_solution
934            .fixed_solution
935            .n_observations
936    } else {
937        solution.rtk_solution.float_solution.n_observations
938    }
939}
940
941fn fix_status_from_mode(mode: StaticReferenceStationMode) -> StaticReferenceFixStatus {
942    match mode {
943        StaticReferenceStationMode::CodeDgnss => StaticReferenceFixStatus::CodeDgnss,
944        StaticReferenceStationMode::CarrierFloat => StaticReferenceFixStatus::CarrierFloat,
945        StaticReferenceStationMode::CarrierFixed => StaticReferenceFixStatus::CarrierFixed,
946    }
947}
948
949fn mode_label(mode: StaticReferenceStationMode) -> &'static str {
950    match mode {
951        StaticReferenceStationMode::CodeDgnss => "code-DGNSS",
952        StaticReferenceStationMode::CarrierFloat => "carrier-float",
953        StaticReferenceStationMode::CarrierFixed => "carrier-fixed",
954    }
955}
956
957fn residuals_rms(values: impl IntoIterator<Item = f64>) -> Option<f64> {
958    let mut sum = 0.0;
959    let mut count = 0usize;
960    for value in values {
961        sum += value * value;
962        count += 1;
963    }
964    (count > 0).then(|| (sum / count as f64).sqrt())
965}
966
967fn failed_report(
968    mode: StaticReferenceStationMode,
969    error: StaticReferenceModeError,
970) -> StaticReferenceModeReport {
971    StaticReferenceModeReport {
972        mode,
973        status: StaticReferenceModeStatus::Failed,
974        used_epochs: 0,
975        skipped_epochs: 0,
976        used_measurements: 0,
977        error: Some(error),
978    }
979}
980
981fn epoch_key(epoch: ObsEpochTime) -> (i32, u8, u8, u8, u8, u64) {
982    (
983        epoch.year,
984        epoch.month,
985        epoch.day,
986        epoch.hour,
987        epoch.minute,
988        epoch.second.to_bits(),
989    )
990}
991
992fn static_reference_input_error(error: validate::FieldError) -> StaticReferenceStationError {
993    StaticReferenceStationError::InvalidInput {
994        field: error.field(),
995        reason: error.reason(),
996    }
997}
sidereon_core/static_reference_station.rs

sidereon_core/
static_reference_station.rs
