sidereon_core/araim/
mod.rs

1//! Advanced RAIM multi-hypothesis snapshot integrity.
2//!
3//! This module is sans-IO: callers provide line-of-sight geometry plus an
4//! externally supplied integrity support message, and the solver returns
5//! protection levels without reading products, global state, or residuals.
6//! ISM records can use the default local pseudorange variance model or direct
7//! per-satellite effective sigmas for reference cases that publish `Cint` and
8//! `Cacc` diagonals.
9
10pub mod fault_modes;
11pub mod ism;
12mod mhss;
13pub mod protection;
14pub mod reliability;
15
16#[cfg(test)]
17mod tests;
18
19pub use fault_modes::{enumerate_fault_modes, FaultHypothesis};
20pub use ism::{ConstellationIsm, Ism, SatelliteIsm, SatelliteIsmModel};
21pub use mhss::{araim, AraimResult, FaultMode};
22pub use protection::ProtectionModel;
23pub use reliability::{
24    reliability_araim, reliability_design, wtest_noncentrality, wtest_noncentrality_components,
25    ObservationReliability, RangeReliabilityRow, ReliabilityOptions, ReliabilityReport,
26    ReliabilitySummary, WtestNoncentralityComponents,
27};
28
29use crate::astro::frames::transforms::geodetic_from_ecef_proj;
30use crate::dop::{ecef_to_enu_rotation, LineOfSight};
31use crate::frame::Wgs84Geodetic;
32use crate::id::{GnssSatelliteId, GnssSystem};
33use crate::spp::{EphemerisSource, ReceiverSolution};
34
35/// One satellite row in an ARAIM geometry snapshot.
36#[derive(Debug, Clone, Copy, PartialEq)]
37pub struct AraimRow {
38    /// Satellite identity used for ISM lookup and satellite-fault modes.
39    pub id: GnssSatelliteId,
40    /// Receiver-to-satellite ECEF unit vector.
41    pub line_of_sight: LineOfSight,
42    /// Constellation owning the signal and constellation-fault mode.
43    pub system: GnssSystem,
44    /// Elevation angle at the receiver, radians.
45    pub elevation_rad: f64,
46}
47
48/// A snapshot geometry and clock-column convention for ARAIM.
49#[derive(Debug, Clone, PartialEq)]
50pub struct AraimGeometry {
51    /// Satellite rows, index-aligned through all gain matrices.
52    pub rows: Vec<AraimRow>,
53    /// Receiver geodetic position for ENU rotation.
54    pub receiver: Wgs84Geodetic,
55    /// Receiver-clock columns, in the same order as the SPP state.
56    pub clock_systems: Vec<GnssSystem>,
57}
58
59impl AraimGeometry {
60    /// Build ARAIM geometry from an SPP solution.
61    ///
62    /// The SPP solution carries the final receiver state and used satellite IDs.
63    /// `t_j2000_s` is the receive epoch used to query the ephemeris source.
64    pub fn from_receiver_solution(
65        solution: &ReceiverSolution,
66        eph: &dyn EphemerisSource,
67        t_j2000_s: f64,
68    ) -> Result<Self, AraimError> {
69        if !t_j2000_s.is_finite() {
70            return Err(AraimError::InsufficientGeometry);
71        }
72        let receiver = match solution.geodetic {
73            Some(receiver) => receiver,
74            None => geodetic_from_position(solution.position.as_array())?,
75        };
76        let clock_systems = receiver_solution_clock_systems(solution)?;
77        if solution.used_sats.len() < 3 + clock_systems.len() {
78            return Err(AraimError::InsufficientGeometry);
79        }
80
81        let rx_ecef_m = solution.position.as_array();
82        let enu = ecef_to_enu_rotation(receiver.lat_rad, receiver.lon_rad);
83        let mut rows = Vec::with_capacity(solution.used_sats.len());
84        for &id in &solution.used_sats {
85            let (sat_ecef_m, _) = eph
86                .position_clock_at_j2000_s(id, t_j2000_s)
87                .ok_or(AraimError::InsufficientGeometry)?;
88            let dx = sat_ecef_m[0] - rx_ecef_m[0];
89            let dy = sat_ecef_m[1] - rx_ecef_m[1];
90            let dz = sat_ecef_m[2] - rx_ecef_m[2];
91            let range_m = (dx * dx + dy * dy + dz * dz).sqrt();
92            if !range_m.is_finite() || range_m <= 0.0 {
93                return Err(AraimError::InsufficientGeometry);
94            }
95
96            let line_of_sight = LineOfSight::new(dx / range_m, dy / range_m, dz / range_m);
97            let up = enu[2][0] * line_of_sight.e_x
98                + enu[2][1] * line_of_sight.e_y
99                + enu[2][2] * line_of_sight.e_z;
100            let elevation_rad = up.clamp(-1.0, 1.0).asin();
101            if !elevation_rad.is_finite() {
102                return Err(AraimError::InsufficientGeometry);
103            }
104
105            rows.push(AraimRow {
106                id,
107                line_of_sight,
108                system: id.system,
109                elevation_rad,
110            });
111        }
112
113        Ok(Self {
114            rows,
115            receiver,
116            clock_systems,
117        })
118    }
119}
120
121fn geodetic_from_position(position_m: [f64; 3]) -> Result<Wgs84Geodetic, AraimError> {
122    let [lon_deg, lat_deg, height_m] =
123        geodetic_from_ecef_proj(position_m[0], position_m[1], position_m[2])
124            .map_err(|_| AraimError::InsufficientGeometry)?;
125    Wgs84Geodetic::new(lat_deg.to_radians(), lon_deg.to_radians(), height_m)
126        .map_err(|_| AraimError::InsufficientGeometry)
127}
128
129fn receiver_solution_clock_systems(
130    solution: &ReceiverSolution,
131) -> Result<Vec<GnssSystem>, AraimError> {
132    let clock_systems = if !solution.system_clocks_s.is_empty() {
133        solution
134            .system_clocks_s
135            .iter()
136            .map(|&(system, _)| system)
137            .collect()
138    } else if !solution.metadata.systems.is_empty() {
139        solution.metadata.systems.clone()
140    } else {
141        crate::spp::clock_systems(&solution.used_sats)
142    };
143    if clock_systems.is_empty() {
144        return Err(AraimError::InsufficientGeometry);
145    }
146    for (idx, system) in clock_systems.iter().enumerate() {
147        if clock_systems[..idx].contains(system) {
148            return Err(AraimError::InsufficientGeometry);
149        }
150    }
151    Ok(clock_systems)
152}
153
154/// Integrity and continuity risk allocation for one ARAIM solve.
155#[derive(Debug, Clone, Copy, PartialEq)]
156pub struct IntegrityAllocation {
157    /// Total probability of hazardous misleading information.
158    pub phmi_total: f64,
159    /// Vertical PHMI allocation.
160    pub phmi_vert: f64,
161    /// Horizontal PHMI allocation.
162    pub phmi_hor: f64,
163    /// Vertical false-alert allocation.
164    pub pfa_vert: f64,
165    /// Horizontal false-alert allocation.
166    pub pfa_hor: f64,
167    /// Maximum acceptable unmonitored fault probability mass.
168    pub p_threshold_unmonitored: f64,
169    /// Fault-prior threshold used for the effective monitor threshold.
170    pub p_emt: f64,
171    /// Maximum enumerated satellite-fault order. Zero keeps only fault-free.
172    pub max_fault_order: usize,
173}
174
175impl IntegrityAllocation {
176    /// LPV-200 allocation from Blanch et al. 2015 and WG-C Milestone 3.
177    pub const fn lpv_200() -> Self {
178        Self {
179            phmi_total: 1.0e-7,
180            phmi_vert: 9.8e-8,
181            phmi_hor: 2.0e-9,
182            pfa_vert: 3.9e-6,
183            pfa_hor: 9.0e-8,
184            // WG-C Reference ADD v3.0 Table 3, LPV-200 PTHRES.
185            p_threshold_unmonitored: 8.0e-8,
186            // WG-C Reference ADD v3.0 Table 2, LPV-200 PEMT.
187            p_emt: 1.0e-5,
188            max_fault_order: 2,
189        }
190    }
191}
192
193/// ARAIM input or numerical failure.
194#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
195pub enum AraimError {
196    /// The full or subset geometry does not have enough independent rows.
197    #[error("insufficient ARAIM geometry")]
198    InsufficientGeometry,
199    /// The unmonitorable fault probability exceeds the allocation.
200    #[error("unmonitorable ARAIM fault probability exceeds allocation")]
201    UnmonitorableFaultMass,
202    /// A matrix operation or root solve failed.
203    #[error("ARAIM numerical failure")]
204    NumericalFailure,
205    /// The ISM is missing, non-finite, or outside its valid domain.
206    #[error("invalid ARAIM ISM")]
207    InvalidIsm,
208    /// The integrity allocation is missing, non-finite, or outside its domain.
209    #[error("invalid ARAIM allocation")]
210    InvalidAllocation,
211}
212
213pub(crate) fn clock_system_for_row(system: GnssSystem) -> GnssSystem {
214    match system {
215        GnssSystem::Sbas => GnssSystem::Gps,
216        other => other,
217    }
218}
219
220pub(crate) fn validate_probability(value: f64, allow_zero: bool) -> bool {
221    value.is_finite()
222        && if allow_zero {
223            (0.0..1.0).contains(&value) || value == 0.0
224        } else {
225            (0.0..1.0).contains(&value)
226        }
227}
228
229pub(crate) fn validate_nonneg_finite(value: f64) -> bool {
230    value.is_finite() && value >= 0.0
231}
232
233pub(crate) fn validate_positive_finite(value: f64) -> bool {
234    value.is_finite() && value > 0.0
235}
sidereon_core/araim/mod.rs

sidereon_core/araim/
mod.rs
