sidereon_core/sbas_pl/
error_model.rs

1//! DO-229 SBAS range-error model.
2
3use crate::araim::{AraimError, AraimRow, ProtectionModel};
4use crate::constants::MEAN_EARTH_RADIUS_KM;
5use crate::dop::ecef_to_enu_rotation;
6use crate::id::{GnssSatelliteId, GnssSystem};
7use crate::ionex::pierce_point;
8use crate::sbas::SbasCorrectionStore;
9pub use crate::sbas::{give_variance_m2_for_givei, udre_variance_m2_for_udrei};
10
11use super::{ProtectionGeometry, SbasPlError};
12
13/// SBAS ionospheric shell height used by DO-229, in kilometers.
14pub const SBAS_IONOSPHERE_SHELL_HEIGHT_KM: f64 = 350.0;
15
16/// One satellite's DO-229 one-sigma range-error budget.
17#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct SbasSisError {
19    /// Satellite identity matching a protection geometry row.
20    pub id: GnssSatelliteId,
21    /// Fast and long-term correction residual sigma, meters.
22    pub sigma_flt_m: f64,
23    /// User ionospheric range-error sigma, meters.
24    pub sigma_uire_m: f64,
25    /// Airborne receiver noise, divergence, and multipath sigma, meters.
26    pub sigma_air_m: f64,
27    /// Tropospheric residual sigma, meters.
28    pub sigma_tropo_m: f64,
29}
30
31impl SbasSisError {
32    /// Sum-of-squares range variance for this satellite, in square meters.
33    pub fn variance_m2(self) -> Option<f64> {
34        if !valid_nonnegative_finite(self.sigma_flt_m)
35            || !valid_nonnegative_finite(self.sigma_uire_m)
36            || !valid_nonnegative_finite(self.sigma_air_m)
37            || !valid_nonnegative_finite(self.sigma_tropo_m)
38        {
39            return None;
40        }
41        let variance_m2 = self.sigma_flt_m * self.sigma_flt_m
42            + self.sigma_uire_m * self.sigma_uire_m
43            + self.sigma_air_m * self.sigma_air_m
44            + self.sigma_tropo_m * self.sigma_tropo_m;
45        (variance_m2.is_finite() && variance_m2 > 0.0).then_some(variance_m2)
46    }
47
48    /// Total one-sigma range error, meters.
49    pub fn sigma_m(self) -> Option<f64> {
50        self.variance_m2().map(f64::sqrt)
51    }
52}
53
54/// Index-aligned SBAS error model for protection-level geometry rows.
55#[derive(Debug, Clone, PartialEq)]
56pub struct SbasErrorModel {
57    /// Per-satellite range-error rows.
58    pub rows: Vec<SbasSisError>,
59}
60
61impl SbasErrorModel {
62    /// Construct an SBAS error model from supplied per-satellite rows.
63    pub fn new(rows: Vec<SbasSisError>) -> Self {
64        Self { rows }
65    }
66
67    /// Build an SBAS error model from decoded SBAS correction storage.
68    ///
69    /// `epoch_j2000_s` is the receive epoch used for freshness checks. Missing
70    /// fast corrections, missing GIVE variance, stale corrections, or invalid
71    /// degradation inputs return [`SbasPlError::InvalidErrorModel`].
72    pub fn from_store(
73        store: &SbasCorrectionStore,
74        geo: GnssSatelliteId,
75        geometry: &ProtectionGeometry,
76        airborne: &AirborneModel,
77        epoch_j2000_s: f64,
78        degradation: &DegradationParams,
79    ) -> Result<Self, SbasPlError> {
80        if geo.system != GnssSystem::Sbas || !epoch_j2000_s.is_finite() || !degradation.is_valid() {
81            return Err(SbasPlError::InvalidErrorModel);
82        }
83        let iono_grid = store
84            .fresh_iono_grid(geo, epoch_j2000_s)
85            .ok_or(SbasPlError::InvalidErrorModel)?;
86        let mut rows = Vec::with_capacity(geometry.rows.len());
87        for row in &geometry.rows {
88            let fast = store
89                .fresh_fast(geo, row.id, epoch_j2000_s)
90                .ok_or(SbasPlError::InvalidErrorModel)?;
91            let sigma_flt_m = sigma_flt_m_for_udrei(fast.udrei, degradation)
92                .ok_or(SbasPlError::InvalidErrorModel)?;
93            let (ipp_lat_deg, ipp_lon_deg, mapping) =
94                pierce_point_for_row(geometry, row).ok_or(SbasPlError::InvalidErrorModel)?;
95            let uive_variance_m2 = iono_grid
96                .variance_at_ipp(ipp_lat_deg, ipp_lon_deg)
97                .ok_or(SbasPlError::InvalidErrorModel)?;
98            let sigma_uire_m = mapping * uive_variance_m2.sqrt() + degradation.eps_iono_m;
99            let sigma_air_m = airborne
100                .sigma_air_m(row.elevation_rad)
101                .ok_or(SbasPlError::InvalidErrorModel)?;
102            let sigma_tropo_m =
103                sigma_tropo_m(row.elevation_rad).ok_or(SbasPlError::InvalidErrorModel)?;
104            let sis = SbasSisError {
105                id: row.id,
106                sigma_flt_m,
107                sigma_uire_m,
108                sigma_air_m,
109                sigma_tropo_m,
110            };
111            if sis.sigma_m().is_none() {
112                return Err(SbasPlError::InvalidErrorModel);
113            }
114            rows.push(sis);
115        }
116        Ok(Self { rows })
117    }
118
119    /// Return the range-error row for one satellite.
120    pub fn row_for(&self, id: GnssSatelliteId) -> Option<&SbasSisError> {
121        self.rows.iter().find(|row| row.id == id)
122    }
123}
124
125impl ProtectionModel for SbasErrorModel {
126    fn sigma_int_m(&self, row: &AraimRow) -> Result<f64, AraimError> {
127        self.row_for(row.id)
128            .and_then(|sis| sis.sigma_m())
129            .ok_or(AraimError::InvalidIsm)
130    }
131
132    fn sigma_acc_m(&self, row: &AraimRow) -> Result<f64, AraimError> {
133        self.sigma_int_m(row)
134    }
135}
136
137/// Airborne receiver and multipath contribution model.
138#[derive(Debug, Clone, Copy, PartialEq)]
139pub struct AirborneModel {
140    /// Receiver noise and code-carrier divergence sigma, meters.
141    pub sigma_noise_divergence_m: f64,
142}
143
144impl AirborneModel {
145    /// Construct an airborne model from a supplied receiver noise term.
146    pub const fn new(sigma_noise_divergence_m: f64) -> Self {
147        Self {
148            sigma_noise_divergence_m,
149        }
150    }
151
152    /// DO-229 AAD-A airborne model.
153    pub const fn aad_a() -> Self {
154        Self::new(0.36)
155    }
156
157    /// Airborne receiver, divergence, and multipath sigma at an elevation.
158    pub fn sigma_air_m(self, elevation_rad: f64) -> Option<f64> {
159        if !valid_nonnegative_finite(self.sigma_noise_divergence_m) {
160            return None;
161        }
162        let sigma_mp_m = sigma_air_multipath_m(elevation_rad)?;
163        let sigma_air_m = (self.sigma_noise_divergence_m * self.sigma_noise_divergence_m
164            + sigma_mp_m * sigma_mp_m)
165            .sqrt();
166        sigma_air_m.is_finite().then_some(sigma_air_m)
167    }
168}
169
170impl Default for AirborneModel {
171    fn default() -> Self {
172        Self::aad_a()
173    }
174}
175
176/// Supplied DO-229 degradation terms not yet decoded from MT10, MT27, or MT28.
177#[derive(Debug, Clone, Copy, PartialEq)]
178pub struct DegradationParams {
179    /// Variance multiplier applied to the UDRE variance table.
180    pub delta_udre: f64,
181    /// Fast-correction degradation term, meters.
182    pub eps_fc_m: f64,
183    /// Range-rate-correction degradation term, meters.
184    pub eps_rrc_m: f64,
185    /// Long-term-correction degradation term, meters.
186    pub eps_ltc_m: f64,
187    /// En-route degradation term, meters.
188    pub eps_er_m: f64,
189    /// Ionospheric degradation term added to UIRE, meters.
190    pub eps_iono_m: f64,
191    /// True when UDRE degradation terms are combined by root-sum-square.
192    pub rss_udre: bool,
193}
194
195impl DegradationParams {
196    /// No extra degradation and no UDRE inflation.
197    pub const fn none() -> Self {
198        Self {
199            delta_udre: 1.0,
200            eps_fc_m: 0.0,
201            eps_rrc_m: 0.0,
202            eps_ltc_m: 0.0,
203            eps_er_m: 0.0,
204            eps_iono_m: 0.0,
205            rss_udre: false,
206        }
207    }
208
209    /// True when every supplied degradation term is finite and non-negative.
210    pub fn is_valid(self) -> bool {
211        valid_positive_finite(self.delta_udre)
212            && valid_nonnegative_finite(self.eps_fc_m)
213            && valid_nonnegative_finite(self.eps_rrc_m)
214            && valid_nonnegative_finite(self.eps_ltc_m)
215            && valid_nonnegative_finite(self.eps_er_m)
216            && valid_nonnegative_finite(self.eps_iono_m)
217    }
218}
219
220impl Default for DegradationParams {
221    fn default() -> Self {
222        Self::none()
223    }
224}
225
226/// Compute fast and long-term residual sigma from a UDREI and degradation terms.
227pub fn sigma_flt_m_for_udrei(udrei: u8, degradation: &DegradationParams) -> Option<f64> {
228    if !degradation.is_valid() {
229        return None;
230    }
231    let udre_variance_m2 = udre_variance_m2_for_udrei(udrei)?;
232    let base_variance_m2 = degradation.delta_udre * udre_variance_m2;
233    let eps_variance_m2 = if degradation.rss_udre {
234        degradation.eps_fc_m * degradation.eps_fc_m
235            + degradation.eps_rrc_m * degradation.eps_rrc_m
236            + degradation.eps_ltc_m * degradation.eps_ltc_m
237            + degradation.eps_er_m * degradation.eps_er_m
238    } else {
239        let eps_m = degradation.eps_fc_m
240            + degradation.eps_rrc_m
241            + degradation.eps_ltc_m
242            + degradation.eps_er_m;
243        eps_m * eps_m
244    };
245    let variance_m2 = base_variance_m2 + eps_variance_m2;
246    (variance_m2.is_finite() && variance_m2 >= 0.0).then_some(variance_m2.sqrt())
247}
248
249/// DO-229 tropospheric residual sigma at an elevation angle, meters.
250pub fn sigma_tropo_m(elevation_rad: f64) -> Option<f64> {
251    if !elevation_rad.is_finite() {
252        return None;
253    }
254    let sin_el = elevation_rad.sin();
255    let sigma_m = 0.12 * 1.001 / (0.002001 + sin_el * sin_el).sqrt();
256    sigma_m.is_finite().then_some(sigma_m)
257}
258
259/// DO-229 SBAS obliquity factor at an elevation angle.
260pub fn sbas_obliquity_factor(elevation_rad: f64) -> Option<f64> {
261    if !elevation_rad.is_finite() {
262        return None;
263    }
264    let s = MEAN_EARTH_RADIUS_KM * elevation_rad.cos()
265        / (MEAN_EARTH_RADIUS_KM + SBAS_IONOSPHERE_SHELL_HEIGHT_KM);
266    let denominator = 1.0 - s * s;
267    (denominator.is_finite() && denominator > 0.0).then_some(1.0 / denominator.sqrt())
268}
269
270/// DO-229 airborne multipath sigma at an elevation angle, meters.
271pub fn sigma_air_multipath_m(elevation_rad: f64) -> Option<f64> {
272    if !elevation_rad.is_finite() {
273        return None;
274    }
275    let elevation_deg = elevation_rad.to_degrees();
276    let sigma_m = 0.13 + 0.53 * (-elevation_deg / 10.0).exp();
277    sigma_m.is_finite().then_some(sigma_m)
278}
279
280fn pierce_point_for_row(geometry: &ProtectionGeometry, row: &AraimRow) -> Option<(f64, f64, f64)> {
281    let azimuth_rad = azimuth_for_row(geometry, row)?;
282    let ipp = pierce_point(
283        geometry.receiver.lat_rad,
284        geometry.receiver.lon_rad,
285        azimuth_rad,
286        row.elevation_rad,
287        MEAN_EARTH_RADIUS_KM,
288        SBAS_IONOSPHERE_SHELL_HEIGHT_KM,
289    );
290    let mapping = sbas_obliquity_factor(row.elevation_rad)?;
291    Some((
292        ipp.phi_ipp_deg,
293        normalize_lon_deg(ipp.lambda_ipp_deg),
294        mapping,
295    ))
296}
297
298fn azimuth_for_row(geometry: &ProtectionGeometry, row: &AraimRow) -> Option<f64> {
299    let los = row.line_of_sight;
300    if !los.e_x.is_finite() || !los.e_y.is_finite() || !los.e_z.is_finite() {
301        return None;
302    }
303    let r = ecef_to_enu_rotation(geometry.receiver.lat_rad, geometry.receiver.lon_rad);
304    let east = r[0][0] * los.e_x + r[0][1] * los.e_y + r[0][2] * los.e_z;
305    let north = r[1][0] * los.e_x + r[1][1] * los.e_y + r[1][2] * los.e_z;
306    let horizontal = east.hypot(north);
307    if !horizontal.is_finite() {
308        return None;
309    }
310    if horizontal <= f64::EPSILON {
311        Some(0.0)
312    } else {
313        Some(east.atan2(north))
314    }
315}
316
317fn valid_nonnegative_finite(value: f64) -> bool {
318    value.is_finite() && value >= 0.0
319}
320
321fn valid_positive_finite(value: f64) -> bool {
322    value.is_finite() && value > 0.0
323}
324
325fn normalize_lon_deg(mut lon_deg: f64) -> f64 {
326    while lon_deg < -180.0 {
327        lon_deg += 360.0;
328    }
329    while lon_deg > 180.0 {
330        lon_deg -= 360.0;
331    }
332    lon_deg
333}
sidereon_core/sbas_pl/error_model.rs

sidereon_core/sbas_pl/
error_model.rs
