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

sidereon_core/araim/
mod.rs
