sidereon_core/araim/
ism.rs

1use std::collections::BTreeSet;
2
3use crate::quality::{pseudorange_variance, PseudorangeVarianceOptions};
4
5use super::{
6    validate_nonneg_finite, validate_positive_finite, validate_probability, AraimError, AraimRow,
7};
8use crate::id::{GnssSatelliteId, GnssSystem};
9
10/// Per-satellite integrity and accuracy model without an identity.
11#[derive(Debug, Clone, Copy, PartialEq)]
12pub struct SatelliteIsmModel {
13    /// Integrity one-sigma SIS range error, meters.
14    pub sigma_ura_m: f64,
15    /// Accuracy and continuity one-sigma SIS range error, meters.
16    pub sigma_ure_m: f64,
17    /// Effective integrity one-sigma range error after local terms, meters.
18    pub effective_sigma_int_m: Option<f64>,
19    /// Effective accuracy one-sigma range error after local terms, meters.
20    pub effective_sigma_acc_m: Option<f64>,
21    /// Nominal SIS bias bound, meters.
22    pub b_nom_m: f64,
23    /// Prior probability for a satellite fault.
24    pub p_sat: f64,
25}
26
27impl SatelliteIsmModel {
28    /// Construct a per-satellite model.
29    pub const fn new(sigma_ura_m: f64, sigma_ure_m: f64, b_nom_m: f64, p_sat: f64) -> Self {
30        Self {
31            sigma_ura_m,
32            sigma_ure_m,
33            effective_sigma_int_m: None,
34            effective_sigma_acc_m: None,
35            b_nom_m,
36            p_sat,
37        }
38    }
39
40    /// Construct a per-satellite model with direct effective range sigmas.
41    pub const fn new_with_effective_sigmas(
42        sigma_ura_m: f64,
43        sigma_ure_m: f64,
44        b_nom_m: f64,
45        p_sat: f64,
46        effective_sigma_int_m: f64,
47        effective_sigma_acc_m: f64,
48    ) -> Self {
49        Self {
50            sigma_ura_m,
51            sigma_ure_m,
52            effective_sigma_int_m: Some(effective_sigma_int_m),
53            effective_sigma_acc_m: Some(effective_sigma_acc_m),
54            b_nom_m,
55            p_sat,
56        }
57    }
58}
59
60/// Per-satellite ISM override.
61#[derive(Debug, Clone, Copy, PartialEq)]
62pub struct SatelliteIsm {
63    /// Satellite identity for this override.
64    pub id: GnssSatelliteId,
65    /// Integrity one-sigma SIS range error, meters.
66    pub sigma_ura_m: f64,
67    /// Accuracy and continuity one-sigma SIS range error, meters.
68    pub sigma_ure_m: f64,
69    /// Effective integrity one-sigma range error after local terms, meters.
70    pub effective_sigma_int_m: Option<f64>,
71    /// Effective accuracy one-sigma range error after local terms, meters.
72    pub effective_sigma_acc_m: Option<f64>,
73    /// Nominal SIS bias bound, meters.
74    pub b_nom_m: f64,
75    /// Prior probability for a satellite fault.
76    pub p_sat: f64,
77}
78
79impl SatelliteIsm {
80    /// Construct a satellite-specific model.
81    pub const fn new(
82        id: GnssSatelliteId,
83        sigma_ura_m: f64,
84        sigma_ure_m: f64,
85        b_nom_m: f64,
86        p_sat: f64,
87    ) -> Self {
88        Self {
89            id,
90            sigma_ura_m,
91            sigma_ure_m,
92            effective_sigma_int_m: None,
93            effective_sigma_acc_m: None,
94            b_nom_m,
95            p_sat,
96        }
97    }
98
99    /// Construct a satellite-specific model with direct effective range sigmas.
100    pub const fn new_with_effective_sigmas(
101        id: GnssSatelliteId,
102        sigma_ura_m: f64,
103        sigma_ure_m: f64,
104        b_nom_m: f64,
105        p_sat: f64,
106        effective_sigma_int_m: f64,
107        effective_sigma_acc_m: f64,
108    ) -> Self {
109        Self {
110            id,
111            sigma_ura_m,
112            sigma_ure_m,
113            effective_sigma_int_m: Some(effective_sigma_int_m),
114            effective_sigma_acc_m: Some(effective_sigma_acc_m),
115            b_nom_m,
116            p_sat,
117        }
118    }
119
120    pub(crate) const fn model(self) -> SatelliteIsmModel {
121        SatelliteIsmModel {
122            sigma_ura_m: self.sigma_ura_m,
123            sigma_ure_m: self.sigma_ure_m,
124            effective_sigma_int_m: self.effective_sigma_int_m,
125            effective_sigma_acc_m: self.effective_sigma_acc_m,
126            b_nom_m: self.b_nom_m,
127            p_sat: self.p_sat,
128        }
129    }
130}
131
132/// Per-constellation fault prior and default satellite model.
133#[derive(Debug, Clone, Copy, PartialEq)]
134pub struct ConstellationIsm {
135    /// Constellation identity.
136    pub system: GnssSystem,
137    /// Prior probability for a constellation-wide fault.
138    pub p_const: f64,
139    /// Default satellite model for rows in this constellation.
140    pub default_sat: SatelliteIsmModel,
141}
142
143impl ConstellationIsm {
144    /// Construct a per-constellation model.
145    pub const fn new(system: GnssSystem, p_const: f64, default_sat: SatelliteIsmModel) -> Self {
146        Self {
147            system,
148            p_const,
149            default_sat,
150        }
151    }
152}
153
154/// Parsed integrity support message used by ARAIM.
155#[derive(Debug, Clone, PartialEq)]
156pub struct Ism {
157    /// Per-constellation defaults and constellation-wide fault priors.
158    pub constellations: Vec<ConstellationIsm>,
159    /// Per-satellite overrides.
160    pub satellites: Vec<SatelliteIsm>,
161}
162
163impl Ism {
164    /// Construct an ISM from parsed records.
165    pub fn new(constellations: Vec<ConstellationIsm>, satellites: Vec<SatelliteIsm>) -> Self {
166        Self {
167            constellations,
168            satellites,
169        }
170    }
171
172    pub(crate) fn validate(&self) -> Result<(), AraimError> {
173        if self.constellations.is_empty() {
174            return Err(AraimError::InvalidIsm);
175        }
176
177        let mut systems = BTreeSet::new();
178        for constellation in &self.constellations {
179            if !systems.insert(constellation.system) {
180                return Err(AraimError::InvalidIsm);
181            }
182            if !validate_probability(constellation.p_const, true)
183                || !validate_sat_model(constellation.default_sat)
184            {
185                return Err(AraimError::InvalidIsm);
186            }
187        }
188
189        let mut satellites = BTreeSet::new();
190        for sat in &self.satellites {
191            if !satellites.insert(sat.id) || !validate_sat_model(sat.model()) {
192                return Err(AraimError::InvalidIsm);
193            }
194            if self.constellation(sat.id.system).is_none() {
195                return Err(AraimError::InvalidIsm);
196            }
197        }
198        Ok(())
199    }
200
201    pub(crate) fn constellation(&self, system: GnssSystem) -> Option<&ConstellationIsm> {
202        self.constellations
203            .iter()
204            .find(|constellation| constellation.system == system)
205    }
206
207    pub(crate) fn model_for(&self, id: GnssSatelliteId) -> Option<SatelliteIsmModel> {
208        self.satellites
209            .iter()
210            .find(|sat| sat.id == id)
211            .map(|sat| sat.model())
212            .or_else(|| self.constellation(id.system).map(|c| c.default_sat))
213    }
214
215    pub(crate) fn effective_for(&self, row: &AraimRow) -> Result<EffectiveIsm, AraimError> {
216        let model = self.model_for(row.id).ok_or(AraimError::InvalidIsm)?;
217        let (sigma_int_m, sigma_acc_m) = if let (Some(sigma_int_m), Some(sigma_acc_m)) =
218            (model.effective_sigma_int_m, model.effective_sigma_acc_m)
219        {
220            (sigma_int_m, sigma_acc_m)
221        } else {
222            let elevation_deg = row.elevation_rad.to_degrees();
223            let local_variance_m2 =
224                pseudorange_variance(elevation_deg, PseudorangeVarianceOptions::default())
225                    .map_err(|_| AraimError::InvalidIsm)?;
226            let sigma_int_m2 = model.sigma_ura_m * model.sigma_ura_m + local_variance_m2;
227            let sigma_acc_m2 = model.sigma_ure_m * model.sigma_ure_m + local_variance_m2;
228            if !validate_positive_finite(sigma_int_m2) || !validate_positive_finite(sigma_acc_m2) {
229                return Err(AraimError::InvalidIsm);
230            }
231            (sigma_int_m2.sqrt(), sigma_acc_m2.sqrt())
232        };
233        if !validate_positive_finite(sigma_int_m)
234            || !validate_positive_finite(sigma_acc_m)
235            || sigma_acc_m > sigma_int_m
236        {
237            return Err(AraimError::InvalidIsm);
238        }
239        Ok(EffectiveIsm {
240            sigma_int_m,
241            sigma_acc_m,
242            b_nom_m: model.b_nom_m,
243            p_sat: model.p_sat,
244        })
245    }
246}
247
248#[derive(Debug, Clone, Copy, PartialEq)]
249pub(crate) struct EffectiveIsm {
250    pub sigma_int_m: f64,
251    pub sigma_acc_m: f64,
252    pub b_nom_m: f64,
253    pub p_sat: f64,
254}
255
256fn validate_sat_model(model: SatelliteIsmModel) -> bool {
257    let valid_effective_sigmas = match (model.effective_sigma_int_m, model.effective_sigma_acc_m) {
258        (Some(sigma_int_m), Some(sigma_acc_m)) => {
259            validate_positive_finite(sigma_int_m)
260                && validate_positive_finite(sigma_acc_m)
261                && sigma_acc_m <= sigma_int_m
262        }
263        (None, None) => true,
264        _ => false,
265    };
266    validate_positive_finite(model.sigma_ura_m)
267        && validate_positive_finite(model.sigma_ure_m)
268        && model.sigma_ure_m <= model.sigma_ura_m
269        && validate_nonneg_finite(model.b_nom_m)
270        && validate_probability(model.p_sat, true)
271        && valid_effective_sigmas
272}
sidereon_core/araim/ism.rs

sidereon_core/araim/
ism.rs
