sidereon_core/araim/
mhss.rs

1use std::collections::BTreeSet;
2
3use super::fault_modes::{enumerate_fault_modes_checked, FaultHypothesis};
4use super::ism::Ism;
5use super::protection::{
6    gain_matrix_enu, gain_matrix_enu_for_clock_systems, k_false_alert, metric_bias, metric_sigma,
7    separation_sigma, solve_protection_level, FalseAlertAxis, ProtectionEquationTerm,
8    ProtectionLevelSolution,
9};
10use super::{
11    clock_system_for_row, validate_probability, AraimError, AraimGeometry, IntegrityAllocation,
12};
13use crate::id::{GnssSatelliteId, GnssSystem};
14
15// WG-C Reference ADD v3.0 Table 3 and Appendix B, Eq. (68): TOLPL is 5e-2 m.
16const HORIZONTAL_PL_TOL_M: f64 = 5.0e-2;
17const VERTICAL_PL_TOL_M: f64 = 1.0e-4;
18
19/// Per-hypothesis ARAIM monitor data.
20#[derive(Debug, Clone, PartialEq)]
21pub struct FaultMode {
22    /// Satellites excluded by this mode.
23    pub excluded: Vec<GnssSatelliteId>,
24    /// Constellation excluded by this mode, if any.
25    pub excluded_constellation: Option<GnssSystem>,
26    /// Fault prior probability for this mode.
27    pub prior: f64,
28    /// Integrity sigma in local `[east, north, up]`, meters.
29    pub sigma_int_enu_m: [f64; 3],
30    /// Nominal bias bound in local `[east, north, up]`, meters.
31    pub bias_enu_m: [f64; 3],
32    /// Separation monitor threshold in local `[east, north, up]`, meters.
33    pub threshold_enu_m: [f64; 3],
34    /// True when the subset geometry is full-rank.
35    pub monitorable: bool,
36}
37
38/// ARAIM protection-level result.
39#[derive(Debug, Clone, PartialEq)]
40pub struct AraimResult {
41    /// Horizontal protection level, meters.
42    pub hpl_m: f64,
43    /// Vertical protection level, meters.
44    pub vpl_m: f64,
45    /// All-in-view horizontal accuracy sigma, meters.
46    pub sigma_acc_h_m: f64,
47    /// All-in-view vertical accuracy sigma, meters.
48    pub sigma_acc_v_m: f64,
49    /// Effective monitor threshold, meters.
50    pub emt_m: f64,
51    /// Per-mode monitor data, including the fault-free mode first.
52    pub fault_modes: Vec<FaultMode>,
53    /// Unenumerated plus unmonitorable fault probability mass.
54    pub p_unmonitored: f64,
55    /// True when the solve met the allocation and all PL roots converged.
56    pub availability: bool,
57}
58
59/// Run the ARAIM MHSS protection-level algorithm.
60pub fn araim(
61    geometry: &AraimGeometry,
62    ism: &Ism,
63    allocation: &IntegrityAllocation,
64) -> Result<AraimResult, AraimError> {
65    validate_geometry(geometry)?;
66    validate_allocation(allocation)?;
67    ism.validate()?;
68
69    let effective = geometry
70        .rows
71        .iter()
72        .map(|row| ism.effective_for(row))
73        .collect::<Result<Vec<_>, _>>()?;
74    let sigma_int_m: Vec<f64> = effective.iter().map(|model| model.sigma_int_m).collect();
75    let sigma_acc_m: Vec<f64> = effective.iter().map(|model| model.sigma_acc_m).collect();
76    let bias_m: Vec<f64> = effective.iter().map(|model| model.b_nom_m).collect();
77    let weights_int: Vec<f64> = sigma_int_m
78        .iter()
79        .map(|sigma| 1.0 / (sigma * sigma))
80        .collect();
81
82    let enumeration = enumerate_fault_modes_checked(geometry, ism, allocation)?;
83    let n_fault_modes = enumeration.modes.len().saturating_sub(1);
84    let k_h = k_false_alert(
85        allocation.pfa_hor,
86        n_fault_modes,
87        FalseAlertAxis::Horizontal,
88    )?;
89    let k_v = k_false_alert(allocation.pfa_vert, n_fault_modes, FalseAlertAxis::Vertical)?;
90
91    let fault_free_int = gain_matrix_enu(geometry, &weights_int)?;
92    let sigma_acc_e = metric_sigma(&fault_free_int.enu_rows[0], &sigma_acc_m);
93    let sigma_acc_n = metric_sigma(&fault_free_int.enu_rows[1], &sigma_acc_m);
94    let sigma_acc_u = metric_sigma(&fault_free_int.enu_rows[2], &sigma_acc_m);
95
96    let mut p_unmonitored = enumeration.p_unenumerated;
97    let mut fault_modes = Vec::with_capacity(enumeration.modes.len());
98    for (mode_idx, hypothesis) in enumeration.modes.iter().enumerate() {
99        let mode = if mode_idx == 0 {
100            compute_monitorable_mode(
101                hypothesis,
102                MonitorInputs {
103                    gain_int: &fault_free_int,
104                    fault_free_int: &fault_free_int,
105                    sigma_int_m: &sigma_int_m,
106                    sigma_acc_m: &sigma_acc_m,
107                    bias_m: &bias_m,
108                    k: [0.0, 0.0, 0.0],
109                },
110            )
111        } else {
112            let weights_int_k = zeroed_weights(geometry, &weights_int, hypothesis);
113            let clock_systems_k = active_clock_systems(geometry, hypothesis);
114            match gain_matrix_enu_for_clock_systems(geometry, &weights_int_k, &clock_systems_k) {
115                Ok(gain_int) => compute_monitorable_mode(
116                    hypothesis,
117                    MonitorInputs {
118                        gain_int: &gain_int,
119                        fault_free_int: &fault_free_int,
120                        sigma_int_m: &sigma_int_m,
121                        sigma_acc_m: &sigma_acc_m,
122                        bias_m: &bias_m,
123                        k: [k_h, k_h, k_v],
124                    },
125                ),
126                _ => {
127                    p_unmonitored += hypothesis.prior;
128                    unmonitorable_mode(hypothesis)
129                }
130            }
131        };
132        fault_modes.push(mode);
133    }
134
135    if p_unmonitored > allocation.p_threshold_unmonitored {
136        return Err(AraimError::UnmonitorableFaultMass);
137    }
138
139    let budget_scale = integrity_budget_scale(allocation, p_unmonitored)?;
140    let pl_e = solve_coord_pl(
141        &fault_modes,
142        0,
143        0.5 * allocation.phmi_hor * budget_scale,
144        HORIZONTAL_PL_TOL_M,
145    )?;
146    let pl_n = solve_coord_pl(
147        &fault_modes,
148        1,
149        0.5 * allocation.phmi_hor * budget_scale,
150        HORIZONTAL_PL_TOL_M,
151    )?;
152    let pl_u = solve_coord_pl(
153        &fault_modes,
154        2,
155        allocation.phmi_vert * budget_scale,
156        VERTICAL_PL_TOL_M,
157    )?;
158    let roots_converged = pl_e.converged && pl_n.converged && pl_u.converged;
159    let emt_m = fault_modes
160        .iter()
161        .filter(|mode| mode.monitorable)
162        .filter(|mode| mode.prior >= allocation.p_emt)
163        .map(|mode| mode.threshold_enu_m[2])
164        .fold(0.0_f64, f64::max);
165    let fault_free_full_rank = fault_modes
166        .first()
167        .map(|mode| mode.monitorable)
168        .unwrap_or(false);
169
170    Ok(AraimResult {
171        hpl_m: (pl_e.value_m * pl_e.value_m + pl_n.value_m * pl_n.value_m).sqrt(),
172        vpl_m: pl_u.value_m,
173        sigma_acc_h_m: (sigma_acc_e * sigma_acc_e + sigma_acc_n * sigma_acc_n).sqrt(),
174        sigma_acc_v_m: sigma_acc_u,
175        emt_m,
176        fault_modes,
177        p_unmonitored,
178        availability: fault_free_full_rank && roots_converged,
179    })
180}
181
182struct MonitorInputs<'a> {
183    gain_int: &'a super::protection::GainMatrix,
184    fault_free_int: &'a super::protection::GainMatrix,
185    sigma_int_m: &'a [f64],
186    sigma_acc_m: &'a [f64],
187    bias_m: &'a [f64],
188    k: [f64; 3],
189}
190
191fn compute_monitorable_mode(hypothesis: &FaultHypothesis, inputs: MonitorInputs<'_>) -> FaultMode {
192    let mut sigma_int_enu_m = [0.0_f64; 3];
193    let mut bias_enu_m = [0.0_f64; 3];
194    let mut threshold_enu_m = [0.0_f64; 3];
195    for coord in 0..3 {
196        sigma_int_enu_m[coord] = metric_sigma(&inputs.gain_int.enu_rows[coord], inputs.sigma_int_m);
197        bias_enu_m[coord] = metric_bias(&inputs.gain_int.enu_rows[coord], inputs.bias_m);
198        threshold_enu_m[coord] = inputs.k[coord]
199            * separation_sigma(
200                &inputs.gain_int.enu_rows[coord],
201                &inputs.fault_free_int.enu_rows[coord],
202                inputs.sigma_acc_m,
203            );
204    }
205
206    FaultMode {
207        excluded: hypothesis.excluded.clone(),
208        excluded_constellation: hypothesis.excluded_constellation,
209        prior: hypothesis.prior,
210        sigma_int_enu_m,
211        bias_enu_m,
212        threshold_enu_m,
213        monitorable: true,
214    }
215}
216
217fn unmonitorable_mode(hypothesis: &FaultHypothesis) -> FaultMode {
218    FaultMode {
219        excluded: hypothesis.excluded.clone(),
220        excluded_constellation: hypothesis.excluded_constellation,
221        prior: hypothesis.prior,
222        sigma_int_enu_m: [f64::INFINITY; 3],
223        bias_enu_m: [f64::INFINITY; 3],
224        threshold_enu_m: [f64::INFINITY; 3],
225        monitorable: false,
226    }
227}
228
229fn zeroed_weights(
230    geometry: &AraimGeometry,
231    weights: &[f64],
232    hypothesis: &FaultHypothesis,
233) -> Vec<f64> {
234    geometry
235        .rows
236        .iter()
237        .zip(weights)
238        .map(|(row, &weight)| {
239            if hypothesis.excludes_satellite(row.id, row.system) {
240                0.0
241            } else {
242                weight
243            }
244        })
245        .collect()
246}
247
248fn active_clock_systems(geometry: &AraimGeometry, hypothesis: &FaultHypothesis) -> Vec<GnssSystem> {
249    geometry
250        .clock_systems
251        .iter()
252        .copied()
253        .filter(|&clock_system| {
254            geometry.rows.iter().any(|row| {
255                !hypothesis.excludes_satellite(row.id, row.system)
256                    && clock_system_for_row(row.system) == clock_system
257            })
258        })
259        .collect()
260}
261
262fn solve_coord_pl(
263    modes: &[FaultMode],
264    coord: usize,
265    integrity_target: f64,
266    tolerance_m: f64,
267) -> Result<ProtectionLevelSolution, AraimError> {
268    let fault_free = modes.first().ok_or(AraimError::NumericalFailure)?;
269    if !fault_free.monitorable {
270        return Err(AraimError::InsufficientGeometry);
271    }
272    let fault_free_term = ProtectionEquationTerm {
273        prior: 1.0,
274        sigma_m: fault_free.sigma_int_enu_m[coord],
275        bias_m: fault_free.bias_enu_m[coord],
276        threshold_m: 0.0,
277    };
278    let fault_terms: Vec<ProtectionEquationTerm> = modes
279        .iter()
280        .skip(1)
281        .filter(|mode| mode.monitorable)
282        .map(|mode| ProtectionEquationTerm {
283            prior: mode.prior,
284            sigma_m: mode.sigma_int_enu_m[coord],
285            bias_m: mode.bias_enu_m[coord],
286            threshold_m: mode.threshold_enu_m[coord],
287        })
288        .collect();
289    solve_protection_level(fault_free_term, &fault_terms, integrity_target, tolerance_m)
290}
291
292fn integrity_budget_scale(
293    allocation: &IntegrityAllocation,
294    p_unmonitored: f64,
295) -> Result<f64, AraimError> {
296    let phmi_split = allocation.phmi_vert + allocation.phmi_hor;
297    let scale = 1.0 - p_unmonitored / phmi_split;
298    if scale > 0.0 && scale.is_finite() {
299        Ok(scale)
300    } else {
301        Err(AraimError::UnmonitorableFaultMass)
302    }
303}
304
305fn validate_geometry(geometry: &AraimGeometry) -> Result<(), AraimError> {
306    if geometry.clock_systems.is_empty() {
307        return Err(AraimError::InsufficientGeometry);
308    }
309    let n_state = 3 + geometry.clock_systems.len();
310    if geometry.rows.len() < n_state {
311        return Err(AraimError::InsufficientGeometry);
312    }
313    let mut clock_systems = BTreeSet::new();
314    for &system in &geometry.clock_systems {
315        if !clock_systems.insert(system) {
316            return Err(AraimError::InsufficientGeometry);
317        }
318    }
319
320    let mut ids = BTreeSet::new();
321    for row in &geometry.rows {
322        if row.id.system != row.system || !ids.insert(row.id) {
323            return Err(AraimError::InsufficientGeometry);
324        }
325        if !(-core::f64::consts::FRAC_PI_2..=core::f64::consts::FRAC_PI_2)
326            .contains(&row.elevation_rad)
327        {
328            return Err(AraimError::InsufficientGeometry);
329        }
330        let los = row.line_of_sight;
331        let norm = (los.e_x * los.e_x + los.e_y * los.e_y + los.e_z * los.e_z).sqrt();
332        if !norm.is_finite() || (norm - 1.0).abs() > 1.0e-3 {
333            return Err(AraimError::InsufficientGeometry);
334        }
335    }
336    Ok(())
337}
338
339fn validate_allocation(allocation: &IntegrityAllocation) -> Result<(), AraimError> {
340    let phmi_split = allocation.phmi_vert + allocation.phmi_hor;
341    let phmi_split_tolerance = allocation.phmi_total * 16.0 * f64::EPSILON;
342    let valid = validate_probability(allocation.phmi_total, false)
343        && validate_probability(allocation.phmi_vert, false)
344        && validate_probability(allocation.phmi_hor, false)
345        && validate_probability(allocation.pfa_vert, false)
346        && validate_probability(allocation.pfa_hor, false)
347        && validate_probability(allocation.p_threshold_unmonitored, true)
348        && validate_probability(allocation.p_emt, false)
349        && phmi_split <= allocation.phmi_total + phmi_split_tolerance;
350    if valid {
351        Ok(())
352    } else {
353        Err(AraimError::InvalidAllocation)
354    }
355}
sidereon_core/araim/mhss.rs

sidereon_core/araim/
mhss.rs
