sidereon_core/
quality.rs

1//! Measurement-quality control for GNSS positioning.
2//!
3//! This module owns the language-independent RAIM/FDE decision logic and the
4//! standard pseudorange weighting primitives used by Sidereon' QC surface.
5
6use std::collections::{BTreeMap, BTreeSet};
7
8use crate::constants::DEG_TO_RAD;
9use crate::spp::{Observation, ReceiverSolution};
10use crate::validate;
11
12/// Default zenith-floor term for pseudorange variance, meters.
13pub const DEFAULT_VARIANCE_A_M: f64 = 0.3;
14/// Default elevation-scaled term for pseudorange variance, meters.
15pub const DEFAULT_VARIANCE_B_M: f64 = 0.3;
16/// Default false-alarm probability for RAIM.
17pub const DEFAULT_P_FA: f64 = 1.0e-3;
18
19/// Pseudorange variance model.
20#[derive(Debug, Clone, Copy, PartialEq, Eq)]
21pub enum PseudorangeVarianceModel {
22    /// Elevation-only `a^2 + b^2 / sin(el)^2`.
23    Elevation,
24    /// Elevation plus a C/N0 variance contribution.
25    ElevationCn0,
26}
27
28/// Options for [`pseudorange_variance`].
29#[derive(Debug, Clone, Copy, PartialEq)]
30pub struct PseudorangeVarianceOptions {
31    /// Zenith-floor term, meters.
32    pub a_m: f64,
33    /// Elevation-scaled term, meters.
34    pub b_m: f64,
35    /// Selected variance model.
36    pub model: PseudorangeVarianceModel,
37    /// Carrier-to-noise density, dB-Hz, required by
38    /// [`PseudorangeVarianceModel::ElevationCn0`].
39    pub cn0_dbhz: Option<f64>,
40    /// C/N0 variance scale, square meters.
41    pub cn0_scale_m2: f64,
42}
43
44impl Default for PseudorangeVarianceOptions {
45    fn default() -> Self {
46        Self {
47            a_m: DEFAULT_VARIANCE_A_M,
48            b_m: DEFAULT_VARIANCE_B_M,
49            model: PseudorangeVarianceModel::Elevation,
50            cn0_dbhz: None,
51            cn0_scale_m2: 1.0,
52        }
53    }
54}
55
56impl PseudorangeVarianceOptions {
57    fn with_entry_cn0(self, cn0_dbhz: f64) -> Self {
58        Self {
59            model: PseudorangeVarianceModel::ElevationCn0,
60            cn0_dbhz: Some(cn0_dbhz),
61            ..self
62        }
63    }
64}
65
66/// One satellite/elevation entry used to build sigma or weight maps.
67#[derive(Debug, Clone, PartialEq)]
68pub struct WeightEntry {
69    /// Satellite token at the binding boundary, e.g. `"G01"`.
70    pub satellite_id: String,
71    /// Topocentric elevation, degrees.
72    pub elevation_deg: f64,
73    /// Optional C/N0 for this observation. When present, it selects the C/N0
74    /// model for this entry.
75    pub cn0_dbhz: Option<f64>,
76}
77
78/// Error from quality-control primitives.
79#[derive(Debug, Clone, Copy, PartialEq, Eq)]
80pub enum QualityError {
81    /// Elevation must be finite, inside `[-90, 90]`, and yield finite variance.
82    InvalidElevation,
83    /// The C/N0 model was selected without a C/N0 value.
84    MissingCn0,
85    /// Variance-model parameters must be finite and non-negative.
86    InvalidParameter,
87    /// Probability must be strictly inside `(0, 1)`.
88    InvalidProbability,
89    /// RAIM system-count override must be positive.
90    InvalidSystemCount,
91    /// Chi-square degrees of freedom must be positive.
92    InvalidDof,
93    /// RAIM weights must be positive finite values.
94    InvalidWeight,
95    /// RAIM residuals must be finite and aligned with used satellites.
96    InvalidResiduals,
97}
98
99impl core::fmt::Display for QualityError {
100    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
101        match self {
102            Self::InvalidElevation => write!(f, "invalid elevation"),
103            Self::MissingCn0 => write!(f, "missing C/N0"),
104            Self::InvalidParameter => write!(f, "invalid quality parameter"),
105            Self::InvalidProbability => write!(f, "invalid probability"),
106            Self::InvalidSystemCount => write!(f, "invalid RAIM system count"),
107            Self::InvalidDof => write!(f, "invalid degrees of freedom"),
108            Self::InvalidWeight => write!(f, "invalid RAIM weight"),
109            Self::InvalidResiduals => write!(f, "invalid RAIM residuals"),
110        }
111    }
112}
113
114impl std::error::Error for QualityError {}
115
116/// Pseudorange measurement variance, square meters.
117pub fn pseudorange_variance(
118    elevation_deg: f64,
119    options: PseudorangeVarianceOptions,
120) -> Result<f64, QualityError> {
121    validate_elevation_deg(elevation_deg)?;
122    validate_variance_options(options)?;
123
124    let mut elevation_var = options.a_m * options.a_m;
125    if options.b_m != 0.0 {
126        let sin_el = (elevation_deg * DEG_TO_RAD).sin();
127        let scaled = options.b_m * options.b_m / (sin_el * sin_el);
128        if !scaled.is_finite() {
129            return Err(QualityError::InvalidElevation);
130        }
131        elevation_var += scaled;
132    }
133
134    let variance = match options.model {
135        PseudorangeVarianceModel::Elevation => elevation_var,
136        PseudorangeVarianceModel::ElevationCn0 => {
137            let Some(cn0) = options.cn0_dbhz else {
138                return Err(QualityError::MissingCn0);
139            };
140            validate_nonneg_parameter(cn0, "cn0_dbhz")?;
141            elevation_var + options.cn0_scale_m2 * 10.0_f64.powf(-cn0 / 10.0)
142        }
143    };
144
145    validate_positive_variance(variance)?;
146    Ok(variance)
147}
148
149fn validate_elevation_deg(elevation_deg: f64) -> Result<(), QualityError> {
150    validate::finite(elevation_deg, "elevation_deg").map_err(|_| QualityError::InvalidElevation)?;
151    if (-90.0..=90.0).contains(&elevation_deg) {
152        Ok(())
153    } else {
154        Err(QualityError::InvalidElevation)
155    }
156}
157
158fn validate_variance_options(options: PseudorangeVarianceOptions) -> Result<(), QualityError> {
159    validate_nonneg_parameter(options.a_m, "variance a_m")?;
160    validate_nonneg_parameter(options.b_m, "variance b_m")?;
161    validate_nonneg_parameter(options.cn0_scale_m2, "variance cn0_scale_m2")
162}
163
164fn validate_nonneg_parameter(value: f64, field: &'static str) -> Result<(), QualityError> {
165    validate::finite_nonneg(value, field)
166        .map(|_| ())
167        .map_err(map_parameter_error)
168}
169
170fn validate_positive_variance(value: f64) -> Result<(), QualityError> {
171    validate::finite_positive(value, "pseudorange variance")
172        .map(|_| ())
173        .map_err(map_parameter_error)
174}
175
176fn map_parameter_error(_error: validate::FieldError) -> QualityError {
177    QualityError::InvalidParameter
178}
179
180/// Build a satellite-to-sigma map. Entries whose variance cannot be computed are
181/// dropped, matching the Sidereon public API.
182pub fn sigmas(
183    entries: &[WeightEntry],
184    options: PseudorangeVarianceOptions,
185) -> BTreeMap<String, f64> {
186    entries
187        .iter()
188        .filter_map(|entry| {
189            let opts = match entry.cn0_dbhz {
190                Some(cn0) => options.with_entry_cn0(cn0),
191                None => options,
192            };
193            pseudorange_variance(entry.elevation_deg, opts)
194                .ok()
195                .map(|var| (entry.satellite_id.clone(), var.sqrt()))
196        })
197        .collect()
198}
199
200/// Build a satellite-to-inverse-variance-weight map. Entries whose variance
201/// cannot be computed are dropped, matching the Sidereon public API.
202pub fn weight_vector(
203    entries: &[WeightEntry],
204    options: PseudorangeVarianceOptions,
205) -> BTreeMap<String, f64> {
206    entries
207        .iter()
208        .filter_map(|entry| {
209            let opts = match entry.cn0_dbhz {
210                Some(cn0) => options.with_entry_cn0(cn0),
211                None => options,
212            };
213            pseudorange_variance(entry.elevation_deg, opts)
214                .ok()
215                .map(|var| (entry.satellite_id.clone(), 1.0 / var))
216        })
217        .collect()
218}
219
220/// RAIM weighting mode.
221#[derive(Debug, Clone, PartialEq)]
222pub enum RaimWeights {
223    /// Unit weights, equivalent to sigma = 1 m for every satellite.
224    Unit,
225    /// Per-satellite inverse variance weights. Missing satellites default to
226    /// unit weight.
227    BySatellite(BTreeMap<String, f64>),
228}
229
230impl RaimWeights {
231    fn validate(&self) -> Result<(), QualityError> {
232        match self {
233            Self::Unit => Ok(()),
234            Self::BySatellite(weights) => weights
235                .values()
236                .try_for_each(|w| validate::finite_positive(*w, "raim weight").map(|_| ()))
237                .map_err(|_| QualityError::InvalidWeight),
238        }
239    }
240
241    fn weight_for(&self, satellite_id: &str) -> f64 {
242        match self {
243            Self::Unit => 1.0,
244            Self::BySatellite(weights) => weights.get(satellite_id).copied().unwrap_or(1.0),
245        }
246    }
247}
248
249/// Options for [`raim`].
250#[derive(Debug, Clone, PartialEq)]
251pub struct RaimOptions {
252    /// False-alarm probability.
253    pub p_fa: f64,
254    /// RAIM residual weights.
255    pub weights: RaimWeights,
256    /// Optional override for the number of distinct GNSS clock systems.
257    pub n_systems: Option<isize>,
258}
259
260impl Default for RaimOptions {
261    fn default() -> Self {
262        Self {
263            p_fa: DEFAULT_P_FA,
264            weights: RaimWeights::Unit,
265            n_systems: None,
266        }
267    }
268}
269
270/// Minimal solution view needed by RAIM.
271#[derive(Debug, Clone, PartialEq)]
272pub struct RaimInput {
273    /// Used satellite tokens, in residual order.
274    pub used_sats: Vec<String>,
275    /// Post-fit pseudorange residuals, meters.
276    pub residuals_m: Vec<f64>,
277}
278
279/// A solution that can feed the RAIM test.
280pub trait RaimSolution {
281    /// Used satellite tokens, in residual order.
282    fn raim_used_sats(&self) -> Vec<String>;
283    /// Post-fit residuals, meters, in used-satellite order.
284    fn raim_residuals_m(&self) -> &[f64];
285}
286
287impl RaimSolution for ReceiverSolution {
288    fn raim_used_sats(&self) -> Vec<String> {
289        self.used_sats.iter().map(ToString::to_string).collect()
290    }
291
292    fn raim_residuals_m(&self) -> &[f64] {
293        &self.residuals_m
294    }
295}
296
297/// Result of a residual chi-square RAIM test.
298#[derive(Debug, Clone, PartialEq)]
299pub struct RaimResult {
300    /// True when the test statistic exceeds the chi-square threshold.
301    pub fault_detected: bool,
302    /// Weighted residual sum of squares.
303    pub test_statistic: f64,
304    /// Chi-square threshold, absent when the geometry is not testable.
305    pub threshold: Option<f64>,
306    /// Degrees of freedom, `n_used - (3 + n_systems)`.
307    pub dof: isize,
308    /// False when `dof <= 0`.
309    pub testable: bool,
310    /// Per-satellite standardized residuals.
311    pub normalized_residuals: BTreeMap<String, f64>,
312    /// Satellite with the largest absolute standardized residual.
313    pub worst_sat: Option<String>,
314}
315
316/// Run RAIM over a generic solution.
317pub fn raim_for_solution<S: RaimSolution>(
318    solution: &S,
319    options: &RaimOptions,
320) -> Result<RaimResult, QualityError> {
321    raim(
322        &RaimInput {
323            used_sats: solution.raim_used_sats(),
324            residuals_m: solution.raim_residuals_m().to_vec(),
325        },
326        options,
327    )
328}
329
330/// Residual-based chi-square RAIM.
331pub fn raim(input: &RaimInput, options: &RaimOptions) -> Result<RaimResult, QualityError> {
332    validate_probability(options.p_fa)?;
333    options.weights.validate()?;
334    validate_raim_input(input)?;
335
336    let n_used = input.used_sats.len() as isize;
337    let n_systems = raim_system_count(input, options)?;
338    let dof = n_used - (3 + n_systems);
339
340    let mut test_statistic = 0.0;
341    let mut normalized_residuals = BTreeMap::new();
342    let mut worst_sat = None::<String>;
343    let mut worst_abs = f64::NEG_INFINITY;
344
345    for (satellite_id, residual_m) in input.used_sats.iter().zip(input.residuals_m.iter()) {
346        let weight = options.weights.weight_for(satellite_id);
347        let normalized = residual_m * weight.sqrt();
348        test_statistic += residual_m * residual_m * weight;
349        normalized_residuals.insert(satellite_id.clone(), normalized);
350        let abs_normalized = normalized.abs();
351        if abs_normalized > worst_abs {
352            worst_abs = abs_normalized;
353            worst_sat = Some(satellite_id.clone());
354        }
355    }
356
357    if dof <= 0 {
358        return Ok(RaimResult {
359            fault_detected: false,
360            test_statistic,
361            threshold: None,
362            dof,
363            testable: false,
364            normalized_residuals,
365            worst_sat,
366        });
367    }
368
369    let threshold = chi2_inv(1.0 - options.p_fa, dof as usize)?;
370    Ok(RaimResult {
371        fault_detected: test_statistic > threshold,
372        test_statistic,
373        threshold: Some(threshold),
374        dof,
375        testable: true,
376        normalized_residuals,
377        worst_sat,
378    })
379}
380
381fn validate_probability(p: f64) -> Result<(), QualityError> {
382    let p = validate::finite(p, "probability").map_err(|_| QualityError::InvalidProbability)?;
383    if p > 0.0 && p < 1.0 {
384        Ok(())
385    } else {
386        Err(QualityError::InvalidProbability)
387    }
388}
389
390fn validate_raim_input(input: &RaimInput) -> Result<(), QualityError> {
391    if input.used_sats.len() != input.residuals_m.len() {
392        return Err(QualityError::InvalidResiduals);
393    }
394    validate::finite_slice(&input.residuals_m, "raim residuals")
395        .map_err(|_| QualityError::InvalidResiduals)
396}
397
398fn raim_system_count(input: &RaimInput, options: &RaimOptions) -> Result<isize, QualityError> {
399    match options.n_systems {
400        Some(n_systems) if n_systems >= 1 => Ok(n_systems),
401        Some(_) => Err(QualityError::InvalidSystemCount),
402        None => Ok(distinct_systems(&input.used_sats)),
403    }
404}
405
406fn distinct_systems(used_sats: &[String]) -> isize {
407    used_sats
408        .iter()
409        .filter_map(|sat| sat.chars().next())
410        .collect::<BTreeSet<_>>()
411        .len() as isize
412}
413
414/// Result of a fault-detection-and-exclusion loop.
415#[derive(Debug, Clone, PartialEq)]
416pub struct FdeResult<S> {
417    /// Final accepted solution.
418    pub solution: S,
419    /// Excluded satellites in exclusion order.
420    pub excluded: Vec<String>,
421    /// Number of exclusions performed.
422    pub iterations: usize,
423}
424
425/// Error from [`fde`].
426#[derive(Debug, Clone, PartialEq)]
427pub enum FdeError<E> {
428    /// RAIM still flagged the set when the exclusion budget was exhausted.
429    FaultUnresolved(f64),
430    /// The supplied solve callback failed.
431    Solve(E),
432    /// RAIM configuration was invalid.
433    Raim(QualityError),
434}
435
436/// Options for [`fde`].
437#[derive(Debug, Clone, PartialEq)]
438pub struct FdeOptions {
439    /// RAIM options used after each solve.
440    pub raim: RaimOptions,
441    /// Maximum number of exclusions to attempt.
442    pub max_iterations: usize,
443}
444
445/// Fault detection and exclusion over a caller-supplied SPP solver.
446pub fn fde<S, E, F>(
447    observations: &[Observation],
448    options: &FdeOptions,
449    mut solve: F,
450) -> Result<FdeResult<S>, FdeError<E>>
451where
452    S: RaimSolution,
453    F: FnMut(&[Observation]) -> Result<S, E>,
454{
455    let mut remaining = observations.to_vec();
456    let mut excluded = Vec::new();
457    let mut iter = 0usize;
458
459    loop {
460        let solution = solve(&remaining).map_err(FdeError::Solve)?;
461        let result = raim_for_solution(&solution, &options.raim).map_err(FdeError::Raim)?;
462
463        if !result.fault_detected {
464            return Ok(FdeResult {
465                solution,
466                excluded,
467                iterations: iter,
468            });
469        }
470
471        let Some(worst) = result.worst_sat else {
472            return Err(FdeError::FaultUnresolved(result.test_statistic));
473        };
474
475        if iter >= options.max_iterations {
476            return Err(FdeError::FaultUnresolved(result.test_statistic));
477        }
478
479        remaining.retain(|ob| ob.satellite_id.to_string() != worst);
480        excluded.push(worst);
481        iter += 1;
482    }
483}
484
485/// Validation policy for receiver solutions returned by SPP.
486#[derive(Debug, Clone, Copy, PartialEq)]
487pub struct SolutionValidationOptions {
488    /// Optional PDOP ceiling.
489    pub max_pdop: Option<f64>,
490    /// Minimum plausible geocentric radius, meters.
491    pub min_plausible_radius_m: f64,
492    /// Maximum plausible geocentric radius, meters.
493    pub max_plausible_radius_m: f64,
494    /// Maximum plausible RMS for a solution flagged converged, meters.
495    pub max_converged_residual_rms_m: f64,
496}
497
498impl Default for SolutionValidationOptions {
499    fn default() -> Self {
500        Self {
501            max_pdop: None,
502            min_plausible_radius_m: 6_344_752.0,
503            max_plausible_radius_m: 8_378_137.0,
504            max_converged_residual_rms_m: 1.0e4,
505        }
506    }
507}
508
509/// Error from [`validate_receiver_solution`].
510#[derive(Debug, Clone, Copy, PartialEq)]
511pub enum SolutionValidationError {
512    /// Validation gate options were malformed or degenerate.
513    InvalidOptions {
514        /// The invalid option field.
515        field: &'static str,
516        /// The validation failure category.
517        reason: &'static str,
518    },
519    /// DOP could not be computed because the geometry was rank deficient.
520    DegenerateGeometryRankDeficient,
521    /// PDOP exceeded the caller's configured ceiling.
522    DegenerateGeometryPdop(f64),
523    /// Position geocentric radius was outside the physical receiver band.
524    ImplausiblePosition(f64),
525    /// Converged solution residuals were non-finite or produced non-finite RMS.
526    InvalidResiduals,
527    /// Converged solution had physically implausible post-fit residual RMS.
528    NoConvergence(f64),
529}
530
531impl core::fmt::Display for SolutionValidationError {
532    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
533        match self {
534            Self::InvalidOptions { field, reason } => {
535                write!(f, "invalid receiver validation option {field}: {reason}")
536            }
537            Self::DegenerateGeometryRankDeficient => {
538                write!(f, "receiver geometry is rank deficient")
539            }
540            Self::DegenerateGeometryPdop(pdop) => {
541                write!(
542                    f,
543                    "receiver geometry PDOP {pdop} exceeds the configured limit"
544                )
545            }
546            Self::ImplausiblePosition(radius_m) => write!(
547                f,
548                "receiver geocentric radius {radius_m} m is outside the plausible range"
549            ),
550            Self::InvalidResiduals => {
551                write!(f, "converged solution residuals must be finite")
552            }
553            Self::NoConvergence(rms_m) => write!(
554                f,
555                "converged solution residual RMS {rms_m} m is implausibly large"
556            ),
557        }
558    }
559}
560
561impl std::error::Error for SolutionValidationError {}
562
563/// Apply the receiver-solution plausibility gates used by the Sidereon SPP API.
564pub fn validate_receiver_solution(
565    solution: &ReceiverSolution,
566    options: SolutionValidationOptions,
567) -> Result<(), SolutionValidationError> {
568    validate_solution_validation_options(options)?;
569
570    let Some(dop) = solution.dop else {
571        return Err(SolutionValidationError::DegenerateGeometryRankDeficient);
572    };
573
574    if let Some(max_pdop) = options.max_pdop {
575        if dop.pdop > max_pdop {
576            return Err(SolutionValidationError::DegenerateGeometryPdop(dop.pdop));
577        }
578    }
579
580    let p = solution.position.as_array();
581    let radius_m = (p[0] * p[0] + p[1] * p[1] + p[2] * p[2]).sqrt();
582    if radius_m < options.min_plausible_radius_m || radius_m > options.max_plausible_radius_m {
583        return Err(SolutionValidationError::ImplausiblePosition(radius_m));
584    }
585
586    if solution.metadata.converged {
587        if validate::finite_slice(&solution.residuals_m, "solution residuals").is_err() {
588            return Err(SolutionValidationError::InvalidResiduals);
589        }
590        let rms = residual_rms(&solution.residuals_m);
591        if !rms.is_finite() {
592            return Err(SolutionValidationError::InvalidResiduals);
593        }
594        if rms > options.max_converged_residual_rms_m {
595            return Err(SolutionValidationError::NoConvergence(rms));
596        }
597    }
598
599    Ok(())
600}
601
602fn validate_solution_validation_options(
603    options: SolutionValidationOptions,
604) -> Result<(), SolutionValidationError> {
605    if let Some(max_pdop) = options.max_pdop {
606        validate::finite_positive(max_pdop, "max_pdop").map_err(validation_option_error)?;
607    }
608    validate::finite_positive(options.min_plausible_radius_m, "min_plausible_radius_m")
609        .map_err(validation_option_error)?;
610    validate::finite_positive(options.max_plausible_radius_m, "max_plausible_radius_m")
611        .map_err(validation_option_error)?;
612    if options.min_plausible_radius_m >= options.max_plausible_radius_m {
613        return Err(invalid_validation_option(
614            "plausible_radius_m",
615            "must be increasing",
616        ));
617    }
618    validate::finite_positive(
619        options.max_converged_residual_rms_m,
620        "max_converged_residual_rms_m",
621    )
622    .map_err(validation_option_error)?;
623    Ok(())
624}
625
626fn validation_option_error(error: validate::FieldError) -> SolutionValidationError {
627    invalid_validation_option(error.field(), error.reason())
628}
629
630fn invalid_validation_option(field: &'static str, reason: &'static str) -> SolutionValidationError {
631    SolutionValidationError::InvalidOptions { field, reason }
632}
633
634fn residual_rms(residuals: &[f64]) -> f64 {
635    if residuals.is_empty() {
636        return 0.0;
637    }
638    let sum_sq = residuals.iter().map(|r| r * r).sum::<f64>();
639    (sum_sq / residuals.len() as f64).sqrt()
640}
641
642/// Chi-square inverse CDF.
643pub fn chi2_inv(p: f64, k: usize) -> Result<f64, QualityError> {
644    validate_probability(p)?;
645    if k == 0 {
646        return Err(QualityError::InvalidDof);
647    }
648    let a = 0.5 * k as f64;
649    let hi0 = (k as f64 + 10.0 * (2.0 * k as f64).sqrt()).max(1.0);
650    let hi = chi2_bracket_hi(p, a, hi0);
651    Ok(chi2_bisect(p, a, 0.0, hi, 0))
652}
653
654fn chi2_bracket_hi(p: f64, a: f64, hi: f64) -> f64 {
655    if chi2_cdf(hi, a) >= p {
656        hi
657    } else {
658        chi2_bracket_hi(p, a, hi * 2.0)
659    }
660}
661
662fn chi2_bisect(p: f64, a: f64, lo: f64, hi: f64, iter: usize) -> f64 {
663    if iter >= 120 {
664        return 0.5 * (lo + hi);
665    }
666    let mid = 0.5 * (lo + hi);
667    if chi2_cdf(mid, a) < p {
668        chi2_bisect(p, a, mid, hi, iter + 1)
669    } else {
670        chi2_bisect(p, a, lo, mid, iter + 1)
671    }
672}
673
674fn chi2_cdf(x: f64, a: f64) -> f64 {
675    regularized_gamma_p(a, 0.5 * x)
676}
677
678const GAMMA_EPS: f64 = 1.0e-15;
679const GAMMA_FPMIN: f64 = 1.0e-300;
680const GAMMA_ITMAX: usize = 1_000;
681
682fn regularized_gamma_p(a: f64, x: f64) -> f64 {
683    if x <= 0.0 {
684        return 0.0;
685    }
686
687    if x < a + 1.0 {
688        let gln = log_gamma(a);
689        let sum = gamma_series(x, 1.0 / a, 1.0 / a, a, 1);
690        sum * (-x + a * x.ln() - gln).exp()
691    } else {
692        let gln = log_gamma(a);
693        let q = gamma_continued_fraction(a, x) * (-x + a * x.ln() - gln).exp();
694        1.0 - q
695    }
696}
697
698fn gamma_series(x: f64, sum: f64, del: f64, ap: f64, n: usize) -> f64 {
699    if n > GAMMA_ITMAX {
700        return sum;
701    }
702    let ap = ap + 1.0;
703    let del = del * x / ap;
704    let sum = sum + del;
705    if del.abs() < sum.abs() * GAMMA_EPS {
706        sum
707    } else {
708        gamma_series(x, sum, del, ap, n + 1)
709    }
710}
711
712fn gamma_continued_fraction(a: f64, x: f64) -> f64 {
713    let b = x + 1.0 - a;
714    let c = 1.0 / GAMMA_FPMIN;
715    let d = 1.0 / safe_denominator(b);
716    gamma_cf_iter(a, b, c, d, d, 1)
717}
718
719fn gamma_cf_iter(a: f64, b: f64, c: f64, d: f64, h: f64, n: usize) -> f64 {
720    if n > GAMMA_ITMAX {
721        return h;
722    }
723
724    let an = -(n as f64) * (n as f64 - a);
725    let b = b + 2.0;
726    let d = 1.0 / safe_denominator(an * d + b);
727    let c = safe_denominator(b + an / c);
728    let delta = d * c;
729    let h = h * delta;
730
731    if (delta - 1.0).abs() < GAMMA_EPS {
732        h
733    } else {
734        gamma_cf_iter(a, b, c, d, h, n + 1)
735    }
736}
737
738fn safe_denominator(x: f64) -> f64 {
739    if x.abs() < GAMMA_FPMIN {
740        GAMMA_FPMIN
741    } else {
742        x
743    }
744}
745
746const LANCZOS: [f64; 9] = [
747    0.9999999999998099,
748    676.5203681218851,
749    -1259.1392167224028,
750    771.3234287776531,
751    -176.6150291621406,
752    12.507343278686905,
753    -0.13857109526572012,
754    9.984369578019572e-6,
755    1.5056327351493116e-7,
756];
757const SQRT_2PI: f64 = 2.5066282746310002;
758
759fn log_gamma(z: f64) -> f64 {
760    if z < 0.5 {
761        std::f64::consts::PI.ln() - (std::f64::consts::PI * z).sin().ln() - log_gamma(1.0 - z)
762    } else {
763        let z = z - 1.0;
764        let mut x = LANCZOS[0];
765        for (i, coef) in LANCZOS.iter().enumerate().skip(1) {
766            x += coef / (z + i as f64);
767        }
768        let t = z + 7.5;
769        SQRT_2PI.ln() + (z + 0.5) * t.ln() - t + x.ln()
770    }
771}
772
773#[cfg(test)]
774mod tests {
775    use super::*;
776    use crate::{GnssSatelliteId, GnssSystem};
777
778    #[derive(Debug, Clone)]
779    struct TestSolution {
780        used_sats: Vec<String>,
781        residuals_m: Vec<f64>,
782    }
783
784    impl RaimSolution for TestSolution {
785        fn raim_used_sats(&self) -> Vec<String> {
786            self.used_sats.clone()
787        }
788
789        fn raim_residuals_m(&self) -> &[f64] {
790            &self.residuals_m
791        }
792    }
793
794    fn gps(prn: u8) -> GnssSatelliteId {
795        GnssSatelliteId::new(GnssSystem::Gps, prn).expect("valid satellite id")
796    }
797
798    fn valid_receiver_solution() -> ReceiverSolution {
799        ReceiverSolution {
800            position: crate::frame::ItrfPositionM::new(6_378_137.0, 0.0, 0.0).unwrap(),
801            geodetic: None,
802            rx_clock_s: 0.0,
803            system_clocks_s: vec![(GnssSystem::Gps, 0.0)],
804            dop: Some(crate::dop::Dop {
805                gdop: 2.5,
806                pdop: 2.0,
807                hdop: 1.5,
808                vdop: 1.0,
809                tdop: 0.5,
810            }),
811            residuals_m: vec![0.1, -0.1, 0.0, 0.05, -0.05],
812            used_sats: (1..=5).map(gps).collect(),
813            rejected_sats: Vec::new(),
814            metadata: crate::spp::SolutionMetadata {
815                iterations: 3,
816                converged: true,
817                status: crate::astro::math::least_squares::Status::StepTolerance,
818                ionosphere_applied: false,
819                troposphere_applied: false,
820                outer_iterations: 0,
821                final_robust_scale_m: None,
822                used_count: 5,
823                systems: vec![GnssSystem::Gps],
824                redundancy: 1,
825                raim_checkable: true,
826            },
827        }
828    }
829
830    #[test]
831    fn pseudorange_variance_matches_elevation_model() {
832        let opts = PseudorangeVarianceOptions::default();
833        let variance = pseudorange_variance(30.0, opts).unwrap();
834        assert!((variance - 0.45).abs() < 1.0e-15);
835        assert_eq!(
836            pseudorange_variance(0.0, opts),
837            Err(QualityError::InvalidElevation)
838        );
839        let horizon_opts = PseudorangeVarianceOptions { b_m: 0.0, ..opts };
840        assert_eq!(
841            pseudorange_variance(0.0, horizon_opts),
842            Ok(horizon_opts.a_m * horizon_opts.a_m)
843        );
844        assert_eq!(
845            pseudorange_variance(-90.0, horizon_opts),
846            Ok(horizon_opts.a_m * horizon_opts.a_m)
847        );
848        assert_eq!(
849            pseudorange_variance(90.1, horizon_opts),
850            Err(QualityError::InvalidElevation)
851        );
852        assert_eq!(
853            pseudorange_variance(f64::NAN, opts),
854            Err(QualityError::InvalidElevation)
855        );
856    }
857
858    #[test]
859    fn cn0_model_requires_cn0_and_adds_noise_term() {
860        let opts = PseudorangeVarianceOptions {
861            model: PseudorangeVarianceModel::ElevationCn0,
862            cn0_dbhz: None,
863            ..Default::default()
864        };
865        assert_eq!(
866            pseudorange_variance(30.0, opts),
867            Err(QualityError::MissingCn0)
868        );
869
870        let weak = pseudorange_variance(
871            30.0,
872            PseudorangeVarianceOptions {
873                cn0_dbhz: Some(30.0),
874                ..opts
875            },
876        )
877        .unwrap();
878        let strong = pseudorange_variance(
879            30.0,
880            PseudorangeVarianceOptions {
881                cn0_dbhz: Some(50.0),
882                ..opts
883            },
884        )
885        .unwrap();
886        assert!(strong < weak);
887    }
888
889    #[test]
890    fn pseudorange_variance_rejects_nonfinite_and_negative_parameters() {
891        let invalid_a = PseudorangeVarianceOptions {
892            a_m: f64::NAN,
893            ..Default::default()
894        };
895        assert_eq!(
896            pseudorange_variance(30.0, invalid_a),
897            Err(QualityError::InvalidParameter)
898        );
899
900        let invalid_b = PseudorangeVarianceOptions {
901            b_m: -1.0,
902            ..Default::default()
903        };
904        assert_eq!(
905            pseudorange_variance(30.0, invalid_b),
906            Err(QualityError::InvalidParameter)
907        );
908
909        let invalid_cn0_scale = PseudorangeVarianceOptions {
910            cn0_scale_m2: f64::INFINITY,
911            ..Default::default()
912        };
913        assert_eq!(
914            pseudorange_variance(30.0, invalid_cn0_scale),
915            Err(QualityError::InvalidParameter)
916        );
917
918        let invalid_cn0 = PseudorangeVarianceOptions {
919            model: PseudorangeVarianceModel::ElevationCn0,
920            cn0_dbhz: Some(f64::NAN),
921            ..Default::default()
922        };
923        assert_eq!(
924            pseudorange_variance(30.0, invalid_cn0),
925            Err(QualityError::InvalidParameter)
926        );
927    }
928
929    #[test]
930    fn pseudorange_variance_rejects_zero_total_variance() {
931        let zero_variance = PseudorangeVarianceOptions {
932            a_m: 0.0,
933            b_m: 0.0,
934            ..Default::default()
935        };
936        assert_eq!(
937            pseudorange_variance(30.0, zero_variance),
938            Err(QualityError::InvalidParameter)
939        );
940
941        let entries = vec![WeightEntry {
942            satellite_id: "G01".to_string(),
943            elevation_deg: 30.0,
944            cn0_dbhz: None,
945        }];
946        let weights = weight_vector(&entries, zero_variance);
947        assert!(
948            !weights.contains_key("G01"),
949            "zero variance must not produce an infinite inverse-variance weight"
950        );
951    }
952
953    #[test]
954    fn sigma_and_weight_maps_drop_invalid_entries() {
955        let entries = vec![
956            WeightEntry {
957                satellite_id: "G01".to_string(),
958                elevation_deg: 90.0,
959                cn0_dbhz: None,
960            },
961            WeightEntry {
962                satellite_id: "G02".to_string(),
963                elevation_deg: -91.0,
964                cn0_dbhz: None,
965            },
966        ];
967        let sigmas = sigmas(&entries, Default::default());
968        let weights = weight_vector(&entries, Default::default());
969        assert!(sigmas.contains_key("G01"));
970        assert!(!sigmas.contains_key("G02"));
971        assert_eq!(weights["G01"], 1.0 / (sigmas["G01"] * sigmas["G01"]));
972    }
973
974    #[test]
975    fn sigma_and_weight_maps_retain_horizon_entries_without_elevation_term() {
976        let entries = vec![
977            WeightEntry {
978                satellite_id: "G01".to_string(),
979                elevation_deg: 0.0,
980                cn0_dbhz: None,
981            },
982            WeightEntry {
983                satellite_id: "G02".to_string(),
984                elevation_deg: f64::NAN,
985                cn0_dbhz: None,
986            },
987        ];
988        let options = PseudorangeVarianceOptions {
989            b_m: 0.0,
990            ..Default::default()
991        };
992        let sigmas = sigmas(&entries, options);
993        let weights = weight_vector(&entries, options);
994        assert_eq!(sigmas["G01"], options.a_m);
995        assert_eq!(weights["G01"], 1.0 / (options.a_m * options.a_m));
996        assert!(!sigmas.contains_key("G02"));
997        assert!(!weights.contains_key("G02"));
998    }
999
1000    #[test]
1001    fn chi_square_inverse_matches_reference_values() {
1002        let refs = [
1003            (1, 10.828),
1004            (2, 13.816),
1005            (3, 16.266),
1006            (4, 18.467),
1007            (5, 20.515),
1008        ];
1009        for (dof, expected) in refs {
1010            let got = chi2_inv(0.999, dof).unwrap();
1011            assert!((got - expected).abs() < 1.0e-3);
1012        }
1013        assert_eq!(chi2_inv(1.0, 1), Err(QualityError::InvalidProbability));
1014        assert_eq!(chi2_inv(0.95, 0), Err(QualityError::InvalidDof));
1015    }
1016
1017    #[test]
1018    fn raim_reports_fault_and_worst_satellite() {
1019        let input = RaimInput {
1020            used_sats: ["G01", "G02", "G03", "G04", "G05"]
1021                .into_iter()
1022                .map(str::to_string)
1023                .collect(),
1024            residuals_m: vec![0.0, 0.0, 0.0, 0.0, 5.0],
1025        };
1026        let result = raim(&input, &RaimOptions::default()).unwrap();
1027        assert!(result.fault_detected);
1028        assert!(result.testable);
1029        assert_eq!(result.dof, 1);
1030        assert_eq!(result.test_statistic, 25.0);
1031        assert_eq!(result.worst_sat.as_deref(), Some("G05"));
1032    }
1033
1034    #[test]
1035    fn raim_dof_zero_is_not_testable() {
1036        let input = RaimInput {
1037            used_sats: ["G01", "G02", "G03", "G04"]
1038                .into_iter()
1039                .map(str::to_string)
1040                .collect(),
1041            residuals_m: vec![0.0, 0.0, 0.0, 0.0],
1042        };
1043        let result = raim(&input, &RaimOptions::default()).unwrap();
1044        assert!(!result.fault_detected);
1045        assert!(!result.testable);
1046        assert_eq!(result.threshold, None);
1047        assert_eq!(result.dof, 0);
1048    }
1049
1050    #[test]
1051    fn raim_rejects_nonpositive_system_overrides() {
1052        let input = RaimInput {
1053            used_sats: ["G01", "G02", "G03", "G04", "G05"]
1054                .into_iter()
1055                .map(str::to_string)
1056                .collect(),
1057            residuals_m: vec![0.0; 5],
1058        };
1059
1060        for n_systems in [0, -1] {
1061            let options = RaimOptions {
1062                n_systems: Some(n_systems),
1063                ..Default::default()
1064            };
1065            assert_eq!(
1066                raim(&input, &options),
1067                Err(QualityError::InvalidSystemCount)
1068            );
1069        }
1070    }
1071
1072    #[test]
1073    fn raim_positive_system_override_controls_dof() {
1074        let input = RaimInput {
1075            used_sats: ["G01", "G02", "G03", "G04", "G05", "G06"]
1076                .into_iter()
1077                .map(str::to_string)
1078                .collect(),
1079            residuals_m: vec![0.0; 6],
1080        };
1081        let options = RaimOptions {
1082            n_systems: Some(2),
1083            ..Default::default()
1084        };
1085
1086        let result = raim(&input, &options).unwrap();
1087        assert!(result.testable);
1088        assert_eq!(result.dof, 1);
1089    }
1090
1091    #[test]
1092    fn raim_rejects_misaligned_or_nonfinite_residuals() {
1093        let input = RaimInput {
1094            used_sats: ["G01", "G02"].into_iter().map(str::to_string).collect(),
1095            residuals_m: vec![1.0],
1096        };
1097        assert_eq!(
1098            raim(&input, &RaimOptions::default()),
1099            Err(QualityError::InvalidResiduals)
1100        );
1101
1102        let input = RaimInput {
1103            used_sats: ["G01", "G02"].into_iter().map(str::to_string).collect(),
1104            residuals_m: vec![1.0, f64::NAN],
1105        };
1106        assert_eq!(
1107            raim(&input, &RaimOptions::default()),
1108            Err(QualityError::InvalidResiduals)
1109        );
1110    }
1111
1112    #[test]
1113    fn raim_rejects_nonfinite_weights_and_probability() {
1114        let input = RaimInput {
1115            used_sats: ["G01", "G02", "G03", "G04", "G05"]
1116                .into_iter()
1117                .map(str::to_string)
1118                .collect(),
1119            residuals_m: vec![0.0; 5],
1120        };
1121        let mut weights = BTreeMap::new();
1122        weights.insert("G01".to_string(), f64::NAN);
1123        let options = RaimOptions {
1124            weights: RaimWeights::BySatellite(weights),
1125            ..Default::default()
1126        };
1127        assert_eq!(raim(&input, &options), Err(QualityError::InvalidWeight));
1128
1129        let options = RaimOptions {
1130            p_fa: f64::NAN,
1131            ..Default::default()
1132        };
1133        assert_eq!(
1134            raim(&input, &options),
1135            Err(QualityError::InvalidProbability)
1136        );
1137    }
1138
1139    #[test]
1140    fn fde_excludes_largest_normalized_residual() {
1141        let observations: Vec<Observation> = (1..=5)
1142            .map(|prn| Observation {
1143                satellite_id: gps(prn),
1144                pseudorange_m: prn as f64,
1145            })
1146            .collect();
1147
1148        let options = FdeOptions {
1149            raim: RaimOptions::default(),
1150            max_iterations: 1,
1151        };
1152        let result = fde(&observations, &options, |remaining| {
1153            let used_sats = remaining
1154                .iter()
1155                .map(|ob| ob.satellite_id.to_string())
1156                .collect::<Vec<_>>();
1157            let residuals_m = remaining
1158                .iter()
1159                .map(|ob| if ob.satellite_id == gps(5) { 5.0 } else { 0.0 })
1160                .collect::<Vec<_>>();
1161            Ok::<_, ()>(TestSolution {
1162                used_sats,
1163                residuals_m,
1164            })
1165        })
1166        .unwrap();
1167
1168        assert_eq!(result.excluded, vec!["G05".to_string()]);
1169        assert_eq!(result.iterations, 1);
1170        assert_eq!(result.solution.used_sats.len(), 4);
1171    }
1172
1173    #[test]
1174    fn fde_refuses_fault_when_budget_is_exhausted() {
1175        let observations: Vec<Observation> = (1..=5)
1176            .map(|prn| Observation {
1177                satellite_id: gps(prn),
1178                pseudorange_m: prn as f64,
1179            })
1180            .collect();
1181        let options = FdeOptions {
1182            raim: RaimOptions::default(),
1183            max_iterations: 0,
1184        };
1185        let err = fde(&observations, &options, |remaining| {
1186            Ok::<_, ()>(TestSolution {
1187                used_sats: remaining
1188                    .iter()
1189                    .map(|ob| ob.satellite_id.to_string())
1190                    .collect(),
1191                residuals_m: vec![0.0, 0.0, 0.0, 0.0, 5.0],
1192            })
1193        })
1194        .unwrap_err();
1195
1196        assert_eq!(err, FdeError::FaultUnresolved(25.0));
1197    }
1198
1199    #[test]
1200    fn receiver_solution_validation_rejects_invalid_gate_options() {
1201        let solution = valid_receiver_solution();
1202        for (options, field, reason) in [
1203            (
1204                SolutionValidationOptions {
1205                    max_pdop: Some(f64::NAN),
1206                    ..Default::default()
1207                },
1208                "max_pdop",
1209                "not finite",
1210            ),
1211            (
1212                SolutionValidationOptions {
1213                    max_pdop: Some(0.0),
1214                    ..Default::default()
1215                },
1216                "max_pdop",
1217                "not positive",
1218            ),
1219            (
1220                SolutionValidationOptions {
1221                    min_plausible_radius_m: 0.0,
1222                    ..Default::default()
1223                },
1224                "min_plausible_radius_m",
1225                "not positive",
1226            ),
1227            (
1228                SolutionValidationOptions {
1229                    max_plausible_radius_m: f64::INFINITY,
1230                    ..Default::default()
1231                },
1232                "max_plausible_radius_m",
1233                "not finite",
1234            ),
1235            (
1236                SolutionValidationOptions {
1237                    max_converged_residual_rms_m: f64::NAN,
1238                    ..Default::default()
1239                },
1240                "max_converged_residual_rms_m",
1241                "not finite",
1242            ),
1243        ] {
1244            assert_eq!(
1245                validate_receiver_solution(&solution, options),
1246                Err(SolutionValidationError::InvalidOptions { field, reason })
1247            );
1248        }
1249
1250        let inverted_radius = SolutionValidationOptions {
1251            min_plausible_radius_m: 8_000_000.0,
1252            max_plausible_radius_m: 7_000_000.0,
1253            ..Default::default()
1254        };
1255        assert_eq!(
1256            validate_receiver_solution(&solution, inverted_radius),
1257            Err(SolutionValidationError::InvalidOptions {
1258                field: "plausible_radius_m",
1259                reason: "must be increasing",
1260            })
1261        );
1262    }
1263
1264    #[test]
1265    fn receiver_solution_validation_rejects_nonfinite_residuals() {
1266        let mut solution = valid_receiver_solution();
1267        solution.residuals_m[1] = f64::NAN;
1268        assert_eq!(
1269            validate_receiver_solution(&solution, SolutionValidationOptions::default()),
1270            Err(SolutionValidationError::InvalidResiduals)
1271        );
1272    }
1273}
sidereon_core/quality.rs

sidereon_core/
quality.rs
