sidereon_core/sbas_pl/
mod.rs

1//! SBAS single-hypothesis protection levels.
2//!
3//! This module is sans-IO. Callers supply snapshot geometry and an externally
4//! supplied range-error model. Message decoding and correction storage remain
5//! in [`crate::sbas`].
6
7pub mod error_model;
8
9pub use crate::araim::{AraimGeometry as ProtectionGeometry, AraimRow as ProtectionRow};
10pub use error_model::{
11    give_variance_m2_for_givei, sbas_obliquity_factor, sigma_air_multipath_m,
12    sigma_flt_m_for_udrei, sigma_tropo_m, udre_variance_m2_for_udrei, AirborneModel,
13    DegradationParams, SbasErrorModel, SbasSisError, SBAS_IONOSPHERE_SHELL_HEIGHT_KM,
14};
15
16use crate::araim::protection::gain_matrix_enu;
17use crate::araim::{AraimError, ProtectionModel};
18use crate::integrity::{error_ellipse_2x2_unit, metric_cross, metric_sigma, IntegrityError};
19
20/// Fixed SBAS protection-level multipliers.
21#[derive(Debug, Clone, Copy, PartialEq)]
22pub struct SbasKMultipliers {
23    /// Horizontal multiplier applied to the horizontal semi-major axis.
24    pub k_h: f64,
25    /// Vertical multiplier applied to the vertical one-sigma standard deviation.
26    pub k_v: f64,
27}
28
29impl SbasKMultipliers {
30    /// DO-229 precision-approach multipliers.
31    pub const PRECISION_APPROACH: Self = Self {
32        k_h: 6.0,
33        k_v: 5.33,
34    };
35
36    /// DO-229 en-route through non-precision-approach multipliers.
37    pub const EN_ROUTE_NPA: Self = Self {
38        k_h: 6.18,
39        k_v: 5.33,
40    };
41}
42
43/// SBAS protection-level output for one geometry snapshot.
44#[derive(Debug, Clone, Copy, PartialEq)]
45pub struct SbasProtection {
46    /// Horizontal protection level, meters.
47    pub hpl_m: f64,
48    /// Vertical protection level, meters.
49    pub vpl_m: f64,
50    /// Horizontal one-sigma semi-major axis, meters.
51    pub d_major_m: f64,
52    /// Vertical one-sigma standard deviation, meters.
53    pub sigma_u_m: f64,
54    /// East one-sigma standard deviation, meters.
55    pub d_east_m: f64,
56    /// North one-sigma standard deviation, meters.
57    pub d_north_m: f64,
58    /// East-north covariance term, square meters.
59    pub d_en_m2: f64,
60}
61
62/// SBAS protection-level input or numerical failure.
63#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
64pub enum SbasPlError {
65    /// The geometry does not have enough independent rows for the active clocks.
66    #[error("insufficient SBAS protection-level geometry")]
67    InsufficientGeometry,
68    /// A matrix operation or covariance projection failed.
69    #[error("SBAS protection-level numerical failure")]
70    NumericalFailure,
71    /// The supplied error model is missing, non-finite, or outside its domain.
72    #[error("invalid SBAS protection-level error model")]
73    InvalidErrorModel,
74}
75
76/// Compute DO-229 SBAS HPL and VPL from geometry and supplied range sigmas.
77///
78/// The protection model supplies one externally determined range sigma per
79/// geometry row. The function forms the same ENU gain matrix used by ARAIM,
80/// projects the diagonal range covariance through that matrix, routes the
81/// horizontal 2x2 covariance through [`error_ellipse_2x2_unit`], and applies the
82/// fixed SBAS K multipliers.
83pub fn sbas_protection_levels(
84    geometry: &ProtectionGeometry,
85    model: &dyn ProtectionModel,
86    k: SbasKMultipliers,
87) -> Result<SbasProtection, SbasPlError> {
88    validate_k(k)?;
89    if geometry.rows.is_empty() {
90        return Err(SbasPlError::InsufficientGeometry);
91    }
92
93    let mut sigmas_m = Vec::with_capacity(geometry.rows.len());
94    for row in &geometry.rows {
95        let sigma_m = model.sigma_int_m(row).map_err(map_model_error)?;
96        if !valid_positive_finite(sigma_m) {
97            return Err(SbasPlError::InvalidErrorModel);
98        }
99        sigmas_m.push(sigma_m);
100    }
101    let weights = sigmas_m
102        .iter()
103        .map(|sigma_m| 1.0 / (sigma_m * sigma_m))
104        .collect::<Vec<_>>();
105    let gain = gain_matrix_enu(geometry, &weights).map_err(map_araim_error)?;
106
107    let d_east_m = metric_sigma(&gain.enu_rows[0], &sigmas_m);
108    let d_north_m = metric_sigma(&gain.enu_rows[1], &sigmas_m);
109    let sigma_u_m = metric_sigma(&gain.enu_rows[2], &sigmas_m);
110    let d_en_m2 = metric_cross(&gain.enu_rows[0], &gain.enu_rows[1], &sigmas_m);
111    if [d_east_m, d_north_m, sigma_u_m, d_en_m2]
112        .iter()
113        .any(|value| !value.is_finite())
114    {
115        return Err(SbasPlError::NumericalFailure);
116    }
117
118    let ellipse = error_ellipse_2x2_unit([
119        [d_east_m * d_east_m, d_en_m2],
120        [d_en_m2, d_north_m * d_north_m],
121    ])
122    .map_err(map_integrity_error)?;
123    let d_major_m = ellipse.semi_major;
124    Ok(SbasProtection {
125        hpl_m: k.k_h * d_major_m,
126        vpl_m: k.k_v * sigma_u_m,
127        d_major_m,
128        sigma_u_m,
129        d_east_m,
130        d_north_m,
131        d_en_m2,
132    })
133}
134
135fn validate_k(k: SbasKMultipliers) -> Result<(), SbasPlError> {
136    if valid_positive_finite(k.k_h) && valid_positive_finite(k.k_v) {
137        Ok(())
138    } else {
139        Err(SbasPlError::InvalidErrorModel)
140    }
141}
142
143fn valid_positive_finite(value: f64) -> bool {
144    value.is_finite() && value > 0.0
145}
146
147fn map_model_error(error: AraimError) -> SbasPlError {
148    match error {
149        AraimError::InsufficientGeometry => SbasPlError::InsufficientGeometry,
150        AraimError::InvalidIsm | AraimError::InvalidAllocation => SbasPlError::InvalidErrorModel,
151        AraimError::UnmonitorableFaultMass | AraimError::NumericalFailure => {
152            SbasPlError::NumericalFailure
153        }
154    }
155}
156
157fn map_araim_error(error: AraimError) -> SbasPlError {
158    match error {
159        AraimError::InsufficientGeometry => SbasPlError::InsufficientGeometry,
160        AraimError::InvalidIsm | AraimError::InvalidAllocation => SbasPlError::InvalidErrorModel,
161        AraimError::UnmonitorableFaultMass | AraimError::NumericalFailure => {
162            SbasPlError::NumericalFailure
163        }
164    }
165}
166
167fn map_integrity_error(error: IntegrityError) -> SbasPlError {
168    match error {
169        IntegrityError::Singular => SbasPlError::InsufficientGeometry,
170        IntegrityError::InvalidInput { .. }
171        | IntegrityError::NonFinite
172        | IntegrityError::NotPositiveSemidefinite
173        | IntegrityError::InvalidProbability { .. } => SbasPlError::NumericalFailure,
174    }
175}
sidereon_core/sbas_pl/mod.rs

sidereon_core/sbas_pl/
mod.rs
