sidereon_core/araim/
protection.rs

1use crate::astro::math::special::{normal_q, normal_q_inv};
2use crate::id::GnssSystem;
3use crate::integrity::{gain_matrix_enu_from_design_rows, IntegrityError};
4pub(crate) use crate::integrity::{metric_sigma, GainMatrix};
5
6use super::{clock_system_for_row, validate_probability, AraimError, AraimGeometry};
7
8/// Supplies per-row range sigmas for ARAIM protection and reliability designs.
9///
10/// Implementations must return externally supplied, positive finite values.
11/// These values are treated as model input. They are never estimated from
12/// residuals or measured ranges by the reliability design path.
13pub trait ProtectionModel {
14    /// Integrity one-sigma range error for a geometry row, meters.
15    fn sigma_int_m(&self, row: &super::AraimRow) -> Result<f64, AraimError>;
16
17    /// Accuracy one-sigma range error for a geometry row, meters.
18    fn sigma_acc_m(&self, row: &super::AraimRow) -> Result<f64, AraimError>;
19}
20
21#[derive(Debug, Clone, Copy, PartialEq)]
22pub(crate) struct ProtectionEquationTerm {
23    pub prior: f64,
24    pub sigma_m: f64,
25    pub bias_m: f64,
26    pub threshold_m: f64,
27}
28
29#[derive(Debug, Clone, Copy, PartialEq)]
30pub(crate) struct ProtectionLevelSolution {
31    pub value_m: f64,
32    pub converged: bool,
33}
34
35#[derive(Debug, Clone, Copy, PartialEq, Eq)]
36pub(crate) enum FalseAlertAxis {
37    Horizontal,
38    Vertical,
39}
40
41pub(crate) fn gain_matrix_enu(
42    geometry: &AraimGeometry,
43    weights: &[f64],
44) -> Result<GainMatrix, AraimError> {
45    gain_matrix_enu_for_clock_systems(geometry, weights, &geometry.clock_systems)
46}
47
48pub(crate) fn gain_matrix_enu_for_clock_systems(
49    geometry: &AraimGeometry,
50    weights: &[f64],
51    clock_systems: &[GnssSystem],
52) -> Result<GainMatrix, AraimError> {
53    if weights.len() != geometry.rows.len() || clock_systems.is_empty() {
54        return Err(AraimError::InsufficientGeometry);
55    }
56    let n_state = 3 + clock_systems.len();
57    if weights.iter().filter(|&&weight| weight > 0.0).count() < n_state {
58        return Err(AraimError::InsufficientGeometry);
59    }
60    if weights
61        .iter()
62        .any(|&weight| !weight.is_finite() || weight < 0.0)
63    {
64        return Err(AraimError::NumericalFailure);
65    }
66
67    let mut design_rows: Vec<Vec<f64>> = Vec::with_capacity(geometry.rows.len());
68    for (row, &weight) in geometry.rows.iter().zip(weights) {
69        let design = if weight > 0.0 {
70            design_row(clock_systems, row)?
71        } else {
72            vec![0.0; n_state]
73        };
74        design_rows.push(design);
75    }
76
77    gain_matrix_enu_from_design_rows(&design_rows, weights, geometry.receiver, n_state)
78        .map_err(map_integrity_error)
79}
80
81pub(crate) fn metric_bias(gain_row: &[f64], biases_m: &[f64]) -> f64 {
82    gain_row
83        .iter()
84        .zip(biases_m)
85        .map(|(&s, &bias)| s.abs() * bias)
86        .sum()
87}
88
89pub(crate) fn separation_sigma(
90    gain_row: &[f64],
91    fault_free_gain_row: &[f64],
92    sigmas_m: &[f64],
93) -> f64 {
94    gain_row
95        .iter()
96        .zip(fault_free_gain_row)
97        .zip(sigmas_m)
98        .map(|((&sk, &s0), &sigma)| {
99            let ds = sk - s0;
100            ds * ds * sigma * sigma
101        })
102        .sum::<f64>()
103        .sqrt()
104}
105
106pub(crate) fn k_false_alert(
107    pfa: f64,
108    n_fault_modes: usize,
109    axis: FalseAlertAxis,
110) -> Result<f64, AraimError> {
111    if n_fault_modes == 0 {
112        return Ok(0.0);
113    }
114    if !validate_probability(pfa, false) {
115        return Err(AraimError::InvalidAllocation);
116    }
117    let denominator_per_mode = match axis {
118        // WG-C Reference ADD v3.0 Eq. (26).
119        FalseAlertAxis::Horizontal => 4.0,
120        // WG-C Reference ADD v3.0 Eq. (27).
121        FalseAlertAxis::Vertical => 2.0,
122    };
123    normal_q_inv(pfa / (denominator_per_mode * n_fault_modes as f64))
124        .ok_or(AraimError::InvalidAllocation)
125}
126
127pub(crate) fn solve_protection_level(
128    fault_free: ProtectionEquationTerm,
129    fault_terms: &[ProtectionEquationTerm],
130    integrity_target: f64,
131    tolerance_m: f64,
132) -> Result<ProtectionLevelSolution, AraimError> {
133    if !validate_probability(integrity_target, false)
134        || tolerance_m <= 0.0
135        || !tolerance_m.is_finite()
136    {
137        return Err(AraimError::InvalidAllocation);
138    }
139    validate_term(fault_free)?;
140    for term in fault_terms {
141        validate_term(*term)?;
142    }
143
144    if fault_terms.is_empty() {
145        let value_m = normal_q_inv(integrity_target * 0.5).ok_or(AraimError::InvalidAllocation)?
146            * fault_free.sigma_m
147            + fault_free.bias_m;
148        return Ok(ProtectionLevelSolution {
149            value_m,
150            converged: true,
151        });
152    }
153
154    let mut lo = 0.0;
155    if protection_lhs(lo, fault_free, fault_terms) <= integrity_target {
156        return Ok(ProtectionLevelSolution {
157            value_m: 0.0,
158            converged: true,
159        });
160    }
161
162    let mut hi = normal_q_inv(integrity_target * 0.5).ok_or(AraimError::InvalidAllocation)?
163        * fault_free.sigma_m
164        + fault_free.bias_m;
165    if hi <= lo || !hi.is_finite() {
166        hi = 1.0;
167    }
168    let mut expanded = 0usize;
169    while protection_lhs(hi, fault_free, fault_terms) > integrity_target {
170        hi *= 2.0;
171        expanded += 1;
172        if !hi.is_finite() || expanded > 100 {
173            return Err(AraimError::NumericalFailure);
174        }
175    }
176
177    let mut converged = false;
178    for _ in 0..80 {
179        let mid = 0.5 * (lo + hi);
180        if protection_lhs(mid, fault_free, fault_terms) > integrity_target {
181            lo = mid;
182        } else {
183            hi = mid;
184        }
185        if hi - lo <= tolerance_m {
186            converged = true;
187            break;
188        }
189    }
190    let value_m = if converged {
191        // Return a conservative PL that is still within the configured root
192        // tolerance. This follows the ADD requirement to output an upper PL
193        // within TOLPL of the equation solution.
194        lo + tolerance_m
195    } else {
196        hi
197    };
198    Ok(ProtectionLevelSolution { value_m, converged })
199}
200
201fn protection_lhs(
202    y_m: f64,
203    fault_free: ProtectionEquationTerm,
204    fault_terms: &[ProtectionEquationTerm],
205) -> f64 {
206    // WG-C Reference ADD v3.0 Eqs. (31)-(32): the fault-free term is two-sided,
207    // and monitored fault terms use the modified Q of Eqs. (7)-(8).
208    let mut value = 2.0 * normal_q((y_m - fault_free.bias_m) / fault_free.sigma_m);
209    for term in fault_terms {
210        value += term.prior * modified_q((y_m - term.threshold_m - term.bias_m) / term.sigma_m);
211    }
212    value
213}
214
215fn modified_q(argument: f64) -> f64 {
216    if argument <= 0.0 {
217        1.0
218    } else {
219        normal_q(argument)
220    }
221}
222
223fn validate_term(term: ProtectionEquationTerm) -> Result<(), AraimError> {
224    if term.prior.is_finite()
225        && term.prior >= 0.0
226        && term.sigma_m.is_finite()
227        && term.sigma_m > 0.0
228        && term.bias_m.is_finite()
229        && term.bias_m >= 0.0
230        && term.threshold_m.is_finite()
231        && term.threshold_m >= 0.0
232    {
233        Ok(())
234    } else {
235        Err(AraimError::NumericalFailure)
236    }
237}
238
239pub(crate) fn design_row(
240    clock_systems: &[GnssSystem],
241    row: &super::AraimRow,
242) -> Result<Vec<f64>, AraimError> {
243    let mut design = vec![0.0_f64; 3 + clock_systems.len()];
244    let los = row.line_of_sight;
245    if !los.e_x.is_finite()
246        || !los.e_y.is_finite()
247        || !los.e_z.is_finite()
248        || !row.elevation_rad.is_finite()
249    {
250        return Err(AraimError::InsufficientGeometry);
251    }
252    design[0] = -los.e_x;
253    design[1] = -los.e_y;
254    design[2] = -los.e_z;
255    let clock_system = clock_system_for_row(row.system);
256    let clock_idx = clock_systems
257        .iter()
258        .position(|&system| system == clock_system)
259        .ok_or(AraimError::InsufficientGeometry)?;
260    design[3 + clock_idx] = 1.0;
261    Ok(design)
262}
263
264fn map_integrity_error(error: IntegrityError) -> AraimError {
265    match error {
266        IntegrityError::Singular => AraimError::InsufficientGeometry,
267        IntegrityError::InvalidInput { .. }
268        | IntegrityError::NonFinite
269        | IntegrityError::NotPositiveSemidefinite
270        | IntegrityError::InvalidProbability { .. } => AraimError::NumericalFailure,
271    }
272}
sidereon_core/araim/protection.rs

sidereon_core/araim/
protection.rs
