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

sidereon_core/araim/
mod.rs
