Skip to main content

sidereon_core/spp/
mod.rs

1//! Single-point positioning (SPP).
2//!
3//! Recovers a receiver ECEF position and clock bias from a set of pseudoranges,
4//! a satellite ephemeris source (a precise SP3 product or a broadcast navigation
5//! message, via the [`EphemerisSource`] trait), and broadcast ionosphere /
6//! Saastamoinen-Niell troposphere correction models. GPS L1 C/A, Galileo E1,
7//! BeiDou B1I, and GLONASS G1 are supported; GPS, BeiDou, and GLONASS use
8//! broadcast Klobuchar coefficients with carrier-frequency scaling, while
9//! Galileo can use its broadcast NeQuick-G `ai0`/`ai1`/`ai2` coefficients when
10//! supplied. GLONASS is FDMA, so its per-satellite carrier is resolved from the
11//! broadcast/observation channel number ([`SolveInputs::glonass_channels`]) and
12//! the Klobuchar L1 delay is scaled to it by `(f_L1 / f_k)^2`, matching
13//! RTKLIB-demo5, which applies no per-satellite inter-frequency bias and carries
14//! the single GLO-GPS offset on the per-system receiver clock. A satellite whose
15//! carrier cannot be resolved is rejected when the ionosphere correction is
16//! requested.
17//!
18//! The state vector is `[x_m, y_m, z_m, clk_0, clk_1, ...]`: three ECEF position
19//! components (meters) followed by one receiver clock per distinct GNSS in the
20//! solve, expressed as a length (meters). A single-system solve reduces to the
21//! classic `[x_m, y_m, z_m, b_m]`; a multi-system solve adds an inter-system
22//! bias parameter for each additional constellation. The seconds value
23//! `rx_clock_s = clk_0 / c` (the reference system) and the per-system clocks are
24//! reported only at the API boundary.
25//!
26//! The per-satellite predicted pseudorange is built in a pinned operation order:
27//! a fixed-count transmit-time iteration (receive time minus geometric range
28//! over `c`) locates the satellite ephemeris at transmission, an Earth-rotation
29//! (Sagnac) closed-form rotation brings the satellite into the receive-time
30//! frame, the geometric range and the line-of-sight azimuth/elevation follow,
31//! then the ionosphere and troposphere delays are added to the predicted range
32//! left-to-right. The residual the solver sees is `sqrt(w) * (P_meas - P_hat)`
33//! with an elevation-based weight evaluated once at the frozen initial-guess
34//! geometry.
35//!
36//! The geometric/clock/correction substrate and its 2-point finite-difference
37//! Jacobian are arithmetic over the libm-bound model functions and are a
38//! bit-exact (0-ULP) parity target against the reference recipe. The converged
39//! position is produced by the trust-region least-squares solver in the
40//! `sidereon-core` solver core, whose linear-algebra step is not bit-reproducible
41//! across BLAS builds; the converged solution is therefore a sub-micron
42//! solver-agreement result, not a 0-ULP claim.
43//!
44//! The bit-exact claim depends on the fused-multiply-add policy matching the
45//! reference exactly. The substrate uses no contracted `a*b+c` anywhere the
46//! reference computes the two roundings separately; the single deliberate
47//! exception is the 3x3-by-vector rotation primitive, which uses `mul_add` to
48//! reproduce the reference's rounding of that product. The certified target
49//! pins `target-cpu`/features so the compiler neither introduces nor drops a
50//! contraction; on a host that auto-contracts these expressions the last bit
51//! can differ and the goldens are not expected to hold.
52
53use crate::astro::math::least_squares::{
54    self, solve_trf_with, LeastSquaresProblem, SolveOptions, Status, TrustRegionSolve,
55};
56use nalgebra::DVector;
57use std::collections::BTreeMap;
58
59mod config;
60mod fallback;
61mod source;
62use crate::astro::math::robust::{huber_weight, mad_scale, RobustError};
63pub use config::{
64    DEFAULT_HUBER_K, DEFAULT_ROBUST_MAX_OUTER, DEFAULT_ROBUST_OUTER_TOL_M,
65    DEFAULT_ROBUST_SCALE_FLOOR_M, ELEVATION_MASK_RAD, SIGMA0_M, TRANSMIT_TIME_ITERATIONS,
66};
67pub use fallback::{
68    solve_broadcast, solve_with_fallback, BroadcastReason, FallbackError, FixSource,
69    SourcedSolution,
70};
71pub use source::EphemerisSource;
72
73pub use crate::constants::{C_M_S, F_L1_HZ, OMEGA_E_DOT_RAD_S};
74use crate::dop::{dop, dop_multi, Dop, LineOfSight};
75use crate::estimation::recipe::{
76    EstimationRecipe, FrameRecipe, RangeRecipe, SagnacRecipe, SolverRecipe,
77};
78use crate::estimation::substrate::frames::{az_el_from_ecef, geodetic_from_ecef};
79use crate::estimation::substrate::parameters::ParameterLayout;
80use crate::estimation::substrate::range::{geometric_range, rotate_transmit_satellite};
81use crate::frame::{ItrfPositionM, Wgs84Geodetic};
82use crate::frequencies;
83use crate::id::{GnssSatelliteId, GnssSystem};
84pub use crate::ionex::GalileoNequickCoeffs;
85use crate::ionex::{
86    galileo_nequick_g_native_unchecked, klobuchar_native_unchecked, GalileoNequickEval,
87    KlobucharParams,
88};
89use crate::quality::{
90    validate_receiver_solution, SolutionValidationError, SolutionValidationOptions,
91};
92use crate::tropo::slant_components;
93use crate::validate;
94
95/// The single-frequency carrier (Hz) the ionosphere correction is reported on
96/// for a constellation with one fixed single-frequency carrier, or `None` for a
97/// system that has none (GLONASS, whose FDMA carrier is per-satellite). GPS L1
98/// C/A and Galileo E1 are both at [`F_L1_HZ`]; BeiDou uses B1I. Klobuchar and
99/// Galileo broadcast delays are reported on this carrier. GLONASS is resolved
100/// per satellite by [`spp_iono_frequency_hz`] from its FDMA channel instead.
101pub(crate) const fn carrier_frequency_hz(system: GnssSystem) -> Option<f64> {
102    frequencies::default_spp_frequency_hz(system)
103}
104
105/// The carrier frequency (Hz) the broadcast ionosphere delay is scaled to for a
106/// single satellite, or `None` if the satellite's system has no carrier the
107/// model can resolve.
108///
109/// For the fixed-carrier systems (GPS L1, Galileo E1, BeiDou B1I) this is the
110/// system carrier from [`carrier_frequency_hz`]. GLONASS is FDMA, so its carrier
111/// is per-satellite: it is resolved from `glonass_channels` (the broadcast /
112/// observation FDMA channel `k` keyed by GLONASS slot number) as the G1
113/// frequency `1602.0 MHz + k * 562.5 kHz`. A GLONASS satellite whose channel is
114/// not in the map, or whose channel is outside the valid FDMA range
115/// `[-7, +6]` (the same domain the RINEX nav/obs parsers enforce via
116/// [`crate::rinex_nav::valid_glonass_frequency_channel`]), has no resolvable
117/// carrier and returns `None` -- `glonass_g1_frequency_hz` is a pure
118/// `1602.0 MHz + k * 562.5 kHz` evaluation that would otherwise return a
119/// bogus-but-positive carrier for an out-of-domain `k`. Mirroring RTKLIB-demo5,
120/// the single GLO-GPS inter-system offset is carried by the existing per-system
121/// receiver clock (see [`clock_systems`]) rather than a separate
122/// inter-frequency-bias parameter, and the only GLONASS-specific term in the
123/// measurement model is this per-satellite `(f_L1 / f_k)^2` ionosphere scaling.
124pub(crate) fn spp_iono_frequency_hz(
125    sat: GnssSatelliteId,
126    glonass_channels: &BTreeMap<u8, i8>,
127) -> Option<f64> {
128    match sat.system {
129        GnssSystem::Glonass => glonass_channels
130            .get(&sat.prn)
131            .copied()
132            .filter(|&k| crate::rinex_nav::valid_glonass_frequency_channel(i32::from(k)))
133            .map(frequencies::glonass_g1_frequency_hz),
134        _ => carrier_frequency_hz(sat.system),
135    }
136}
137use crate::constants::MEAN_EARTH_RADIUS_M;
138const PI: f64 = std::f64::consts::PI;
139
140// Agreement-track stopping thresholds for the independent SPP least-squares
141// solver. These drive the solver to the true fixed point of the noise-free,
142// by-construction-zero-residual problem so the converged position agrees with
143// the reference solution to the documented sub-micron bound; they are the
144// solver's own stopping thresholds, not a parity target's pinned scipy options.
145/// Canonical light-time convergence tolerance (s). The canonical range recipe
146/// ([`RangeRecipe::CanonicalLightTimeClosedFormSagnac`]) iterates the
147/// transmit-epoch light-time loop until the signal travel time changes by less
148/// than this between iterations, instead of the reference recipe's fixed
149/// [`TRANSMIT_TIME_ITERATIONS`] truncation. `1e-13 s` is ~30 microns of range
150/// (`tol * C_M_S`), far below the pseudorange noise floor; the loop is
151/// quadratically convergent so it reaches this in ~3 iterations.
152const CANONICAL_LIGHT_TIME_TOL_S: f64 = 1.0e-13;
153/// Iteration cap for the canonical light-time loop, a safety bound the
154/// quadratically convergent iteration never reaches in practice (it converges in
155/// ~3 iterations); present so a pathological geometry cannot spin forever.
156const CANONICAL_LIGHT_TIME_MAX_ITERS: usize = 10;
157/// First-order optimality tolerance on `||J^T r||_inf`.
158const SPP_SOLVER_GTOL: f64 = 1e-14;
159/// Relative-cost-reduction tolerance.
160const SPP_SOLVER_FTOL: f64 = 1e-15;
161/// Relative-step tolerance.
162const SPP_SOLVER_XTOL: f64 = 1e-14;
163/// Maximum number of residual evaluations.
164const SPP_SOLVER_MAX_NFEV: usize = 400;
165
166/// A single GPS L1 pseudorange observation.
167///
168/// The input boundary of the pipeline is the pseudorange; raw observation
169/// formation (RINEX decoding, code tracking) is out of scope. The receive epoch
170/// and the time-of-day / day-of-year arguments are common to all observations
171/// in one solve and are carried on [`SolveInputs`], not here.
172#[derive(Debug, Clone, Copy, PartialEq)]
173pub struct Observation {
174    /// The transmitting satellite.
175    pub satellite_id: GnssSatelliteId,
176    /// Measured pseudorange in meters.
177    pub pseudorange_m: f64,
178}
179
180/// Why a satellite was excluded from the solve, in pinned priority order.
181#[derive(Debug, Clone, Copy, PartialEq, Eq)]
182pub enum RejectionReason {
183    /// The SP3 product has no usable position or clock for the satellite at the
184    /// transmit epoch.
185    NoEphemeris,
186    /// The satellite is below the elevation mask at the frozen geometry.
187    LowElevation,
188}
189
190/// A rejected satellite paired with its rejection reason.
191#[derive(Debug, Clone, Copy, PartialEq, Eq)]
192pub struct RejectedSat {
193    /// The excluded satellite.
194    pub satellite_id: GnssSatelliteId,
195    /// The first matching rejection reason.
196    pub reason: RejectionReason,
197}
198
199/// Models and convergence detail describing how a solution was produced.
200#[derive(Debug, Clone, PartialEq)]
201pub struct SolutionMetadata {
202    /// Number of accepted solver iterations.
203    pub iterations: usize,
204    /// Whether the solver reached a convergence stopping criterion (as opposed
205    /// to exhausting its evaluation budget).
206    pub converged: bool,
207    /// The solver's termination status.
208    pub status: Status,
209    /// Whether the ionosphere correction was applied.
210    pub ionosphere_applied: bool,
211    /// Whether the troposphere correction was applied.
212    pub troposphere_applied: bool,
213    /// Number of outer robust-reweighting iterations performed. `0` on the
214    /// static path (`robust = None`); on the robust path this counts the
215    /// reweighted resolves beyond the warm-start solve.
216    pub outer_iterations: usize,
217    /// The final MAD robust scale (m) of the last outer iteration, or `None` on
218    /// the static path.
219    pub final_robust_scale_m: Option<f64>,
220    /// Number of satellites used in the final solve.
221    pub used_count: usize,
222    /// Distinct GNSS systems present in the final solve, in ascending order.
223    pub systems: Vec<GnssSystem>,
224    /// Degrees of freedom, `used_count - (3 + systems.len())`.
225    pub redundancy: isize,
226    /// Whether residual-based RAIM can test the final solve (`redundancy >= 1`).
227    pub raim_checkable: bool,
228}
229
230/// A receiver position/clock solution with its geometry diagnostics.
231#[derive(Debug, Clone)]
232pub struct ReceiverSolution {
233    /// Converged receiver position, ITRF/IGS ECEF meters.
234    pub position: ItrfPositionM,
235    /// The geodetic form of the position, if the conversion was requested.
236    pub geodetic: Option<Wgs84Geodetic>,
237    /// Receiver clock bias in seconds (`clk_0 / c`) for the reference GNSS - the
238    /// first entry of `system_clocks_s`. For a single-system solve this is the
239    /// only clock; for a multi-system solve the other systems' absolute clocks
240    /// are in `system_clocks_s`.
241    pub rx_clock_s: f64,
242    /// The absolute receiver clock for each GNSS in the solve, in ascending
243    /// system order, in seconds. One entry for a single-system solve; one per
244    /// constellation for a multi-system solve. The first entry equals
245    /// `rx_clock_s`; the inter-system bias for any other system is *its clock
246    /// minus that reference* (these are absolute per-system clocks, not biases).
247    pub system_clocks_s: Vec<(GnssSystem, f64)>,
248    /// Dilution-of-precision scalars from the converged geometry. A
249    /// single-system solve uses the 0-ULP four-state cofactor; a multi-system
250    /// solve uses the general inverse with one clock column per constellation (a
251    /// deterministic diagnostic, not a 0-ULP target). `None` only if the
252    /// converged geometry is rank-deficient.
253    pub dop: Option<Dop>,
254    /// Post-fit residuals in meters, in `used_sats` order (unweighted
255    /// `P_meas - P_hat`).
256    pub residuals_m: Vec<f64>,
257    /// The satellites that contributed to the solve, ascending id order.
258    pub used_sats: Vec<GnssSatelliteId>,
259    /// The excluded satellites, each with its reason.
260    pub rejected_sats: Vec<RejectedSat>,
261    /// Iteration / convergence / model metadata.
262    pub metadata: SolutionMetadata,
263}
264
265/// Which correction terms a solve applies, building up incrementally.
266#[derive(Debug, Clone, Copy, PartialEq, Eq)]
267pub struct Corrections {
268    /// Apply the Klobuchar L1 ionosphere delay.
269    pub ionosphere: bool,
270    /// Apply the Saastamoinen/Niell troposphere delay.
271    pub troposphere: bool,
272}
273
274impl Corrections {
275    /// No atmospheric corrections (geometry + clock + Sagnac only).
276    pub const NONE: Self = Self {
277        ionosphere: false,
278        troposphere: false,
279    };
280    /// Ionosphere only.
281    pub const IONO: Self = Self {
282        ionosphere: true,
283        troposphere: false,
284    };
285    /// Ionosphere and troposphere.
286    pub const IONO_TROPO: Self = Self {
287        ionosphere: true,
288        troposphere: true,
289    };
290}
291
292/// Broadcast Klobuchar coefficients for the ionosphere term.
293#[derive(Debug, Clone, Copy, PartialEq)]
294pub struct KlobucharCoeffs {
295    /// Cosine-amplitude polynomial coefficients (a0..a3).
296    pub alpha: [f64; 4],
297    /// Period polynomial coefficients (b0..b3).
298    pub beta: [f64; 4],
299}
300
301/// Surface meteorology for the troposphere term.
302#[derive(Debug, Clone, Copy, PartialEq)]
303pub struct SurfaceMet {
304    /// Total pressure (hPa).
305    pub pressure_hpa: f64,
306    /// Temperature (K).
307    pub temperature_k: f64,
308    /// Relative humidity, fraction in `[0, 1]`.
309    pub relative_humidity: f64,
310}
311
312/// Opt-in Huber/IRLS robust-reweighting configuration.
313///
314/// When a [`SolveInputs::robust`] is `Some(_)`, the solve runs an outer
315/// iteratively-reweighted least-squares loop on top of the static elevation
316/// weighting: a warm-start solve at the base elevation weights (bit-identical to
317/// the static path), then re-solves that rebuild the weight vector each outer
318/// iteration as `base_elevation_weight * huber(r_i / s)`, where `r_i` is the
319/// current unweighted post-fit residual and `s` is a floored MAD scale. With
320/// `robust = None` the solve is byte-identical to the static elevation-weighted
321/// solve. `Default` matches the `DEFAULT_*` config constants.
322#[derive(Debug, Clone, Copy, PartialEq)]
323pub struct RobustConfig {
324    /// Huber tuning constant `k`; residuals scaled below this keep full weight.
325    pub huber_k: f64,
326    /// Floor (m) on the MAD scale, preventing a near-perfect fit from
327    /// down-weighting every satellite.
328    pub scale_floor_m: f64,
329    /// Maximum total outer solves (the warm start plus reweighted resolves).
330    pub max_outer: usize,
331    /// Outer-loop position L2 step tolerance (m).
332    pub outer_tol_m: f64,
333}
334
335impl Default for RobustConfig {
336    fn default() -> Self {
337        Self {
338            huber_k: DEFAULT_HUBER_K,
339            scale_floor_m: DEFAULT_ROBUST_SCALE_FLOOR_M,
340            max_outer: DEFAULT_ROBUST_MAX_OUTER,
341            outer_tol_m: DEFAULT_ROBUST_OUTER_TOL_M,
342        }
343    }
344}
345
346/// Everything one SPP solve needs besides the SP3 product itself.
347///
348/// The receive epoch is carried as seconds-since-J2000 (`t_rx_j2000_s`), the
349/// argument the transmit-time iteration differences against the geometric range
350/// to land the satellite ephemeris at transmission, with no Julian-date
351/// round-trip inside the loop. The Klobuchar diurnal argument
352/// (`t_rx_second_of_day_s`) and the Niell seasonal argument (`day_of_year`) are
353/// supplied directly so the correction kernels run in their bit-exact native
354/// units.
355#[derive(Debug, Clone)]
356pub struct SolveInputs {
357    /// The pseudorange observations (any order; the solve sorts them).
358    pub observations: Vec<Observation>,
359    /// Receive epoch, seconds since J2000 in the SP3 product's time scale.
360    pub t_rx_j2000_s: f64,
361    /// GPS second-of-day of the receive epoch (Klobuchar diurnal argument).
362    pub t_rx_second_of_day_s: f64,
363    /// Fractional day-of-year of the receive epoch (Niell seasonal argument).
364    pub day_of_year: f64,
365    /// Initial guess `[x_m, y_m, z_m, b_m]`.
366    pub initial_guess: [f64; 4],
367    /// The correction terms to apply.
368    pub corrections: Corrections,
369    /// Broadcast Klobuchar coefficients (used iff `corrections.ionosphere`).
370    /// Applied to every system unless `beidou_klobuchar` overrides BeiDou.
371    pub klobuchar: KlobucharCoeffs,
372    /// Optional BeiDou-specific Klobuchar coefficients (the broadcast `BDSA`/
373    /// `BDSB` set). When present, BeiDou satellites use these instead of
374    /// [`klobuchar`](Self::klobuchar); both feed the same model, frequency-scaled
375    /// to B1I. `None` falls back to `klobuchar` for BeiDou too.
376    pub beidou_klobuchar: Option<KlobucharCoeffs>,
377    /// Optional Galileo-specific NeQuick-G coefficients (the broadcast `GAL`
378    /// `ai0`/`ai1`/`ai2` set). When present, Galileo satellites use these instead
379    /// of the GPS Klobuchar coefficients. `None` preserves the historical
380    /// Klobuchar fallback so existing zero-Galileo goldens stay bit-identical.
381    pub galileo_nequick: Option<GalileoNequickCoeffs>,
382    /// GLONASS FDMA channel numbers keyed by GLONASS slot (PRN), from the
383    /// broadcast nav `freq_channel` field or the observation header's
384    /// `GLONASS SLOT / FRQ #` records. Used only to resolve the per-satellite
385    /// GLONASS carrier for the ionosphere `(f_L1 / f_k)^2` scaling; an empty map
386    /// is correct for any solve with no GLONASS observation and leaves every
387    /// other constellation bit-identical. A GLONASS observation with the
388    /// ionosphere correction requested but no channel here is rejected with
389    /// [`SppError::IonosphereUnsupported`].
390    pub glonass_channels: BTreeMap<u8, i8>,
391    /// Surface meteorology (used iff `corrections.troposphere`).
392    pub met: SurfaceMet,
393    /// Opt-in Huber/IRLS robust reweighting. `None` (the default behavior)
394    /// runs the static elevation-weighted solve byte-identically; `Some(_)`
395    /// adds the outer reweighting loop described on [`RobustConfig`].
396    pub robust: Option<RobustConfig>,
397}
398
399/// Input-validation failure category for SPP public entry points.
400#[derive(Debug, Clone, Copy, PartialEq, Eq)]
401pub enum SppInputErrorKind {
402    /// A floating-point input was NaN or infinite.
403    NonFinite,
404    /// A positive physical input was zero or negative.
405    NotPositive,
406    /// A non-negative physical input was negative.
407    Negative,
408    /// A finite numeric input was outside its accepted range.
409    OutOfRange,
410    /// A required input field was absent.
411    Missing,
412    /// A text field could not be parsed as a float.
413    FloatParse,
414    /// A text field could not be parsed as an integer.
415    IntParse,
416    /// A civil date field was out of range.
417    InvalidCivilDate,
418    /// A civil time field was out of range.
419    InvalidCivilTime,
420}
421
422impl core::fmt::Display for SppInputErrorKind {
423    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
424        let label = match self {
425            Self::NonFinite => "not finite",
426            Self::NotPositive => "not positive",
427            Self::Negative => "negative",
428            Self::OutOfRange => "out of range",
429            Self::Missing => "missing",
430            Self::FloatParse => "invalid float",
431            Self::IntParse => "invalid integer",
432            Self::InvalidCivilDate => "invalid civil date",
433            Self::InvalidCivilTime => "invalid civil time",
434        };
435        f.write_str(label)
436    }
437}
438
439impl From<&validate::FieldError> for SppInputErrorKind {
440    fn from(error: &validate::FieldError) -> Self {
441        match error {
442            validate::FieldError::Missing { .. } => Self::Missing,
443            validate::FieldError::NonFinite { .. } => Self::NonFinite,
444            validate::FieldError::NotPositive { .. } => Self::NotPositive,
445            validate::FieldError::Negative { .. } => Self::Negative,
446            validate::FieldError::OutOfRange { .. } => Self::OutOfRange,
447            validate::FieldError::FloatParse { .. } => Self::FloatParse,
448            validate::FieldError::IntParse { .. } => Self::IntParse,
449            validate::FieldError::InvalidCivilDate { .. } => Self::InvalidCivilDate,
450            validate::FieldError::InvalidCivilTime { .. } => Self::InvalidCivilTime,
451        }
452    }
453}
454
455/// Error from [`solve`].
456#[derive(Debug, Clone)]
457pub enum SppError {
458    /// A public SPP input was malformed, non-finite, or outside its physical
459    /// domain. Boundary validation rejects this before satellite selection or
460    /// least-squares evaluation.
461    InvalidInput {
462        /// The invalid input field.
463        field: &'static str,
464        /// The validation failure category.
465        kind: SppInputErrorKind,
466    },
467    /// Fewer usable satellites survived rejection than the solve has parameters
468    /// (`3 + n_systems`: three position components plus one receiver clock per
469    /// GNSS), so the solve is underdetermined.
470    TooFewSatellites {
471        /// The number of satellites that survived rejection.
472        used: usize,
473        /// The number of satellites required (`3 + n_systems`).
474        required: usize,
475    },
476    /// The trust-region step hit a rank-deficient Jacobian (degenerate geometry).
477    Singular(least_squares::SolveError),
478    /// The same satellite appears in more than one observation. One pseudorange
479    /// per satellite is required, so the input is rejected rather than silently
480    /// picking one (which would make the result depend on observation order).
481    DuplicateObservation {
482        /// The satellite that was observed more than once.
483        satellite: GnssSatelliteId,
484    },
485    /// A satellite that survived the frozen selection had no usable SP3
486    /// position/clock at a transmit epoch reached during the solve. Returned
487    /// instead of panicking; normally precluded by the selection step.
488    EphemerisLost {
489        /// The satellite whose ephemeris became unavailable during the solve.
490        satellite: GnssSatelliteId,
491    },
492    /// The ionosphere correction was requested but an observed satellite has no
493    /// resolvable carrier frequency, so the L1 Klobuchar delay cannot be scaled
494    /// to it. GPS L1, Galileo E1, and BeiDou B1I have fixed carriers; a GLONASS
495    /// satellite resolves its per-satellite FDMA carrier from
496    /// [`SolveInputs::glonass_channels`], so a GLONASS observation whose channel
497    /// is absent from that map -- or present but outside the valid FDMA range
498    /// `[-7, +6]` -- (rather than GLONASS as a whole) is rejected here rather
499    /// than corrected with an undefined or out-of-domain frequency.
500    IonosphereUnsupported {
501        /// The satellite the ionosphere model does not cover.
502        satellite: GnssSatelliteId,
503    },
504}
505
506impl core::fmt::Display for SppError {
507    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
508        match self {
509            SppError::InvalidInput { field, kind } => {
510                write!(f, "invalid SPP input {field}: {kind}")
511            }
512            SppError::TooFewSatellites { used, required } => write!(
513                f,
514                "only {used} usable satellites; need at least {required} \
515                 (3 position + 1 clock per GNSS)"
516            ),
517            SppError::Singular(e) => write!(f, "degenerate geometry: {e}"),
518            SppError::DuplicateObservation { satellite } => {
519                write!(f, "satellite {satellite} observed more than once")
520            }
521            SppError::EphemerisLost { satellite } => {
522                write!(f, "satellite {satellite} lost ephemeris during the solve")
523            }
524            SppError::IonosphereUnsupported { satellite } => write!(
525                f,
526                "ionosphere correction has no modeled carrier frequency for {satellite}"
527            ),
528        }
529    }
530}
531
532impl std::error::Error for SppError {
533    fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
534        match self {
535            SppError::Singular(error) => Some(error),
536            _ => None,
537        }
538    }
539}
540
541impl From<least_squares::SolveError> for SppError {
542    fn from(e: least_squares::SolveError) -> Self {
543        SppError::Singular(e)
544    }
545}
546
547/// Language-independent SPP solve policy used by the public API boundary.
548#[derive(Debug, Clone, Copy, Default, PartialEq)]
549pub struct SolvePolicy {
550    /// Business-level solution validation gates.
551    pub validation: SolutionValidationOptions,
552    /// Optional count of near-surface golden-spiral seeds for cold starts.
553    pub coarse_search_seeds: Option<usize>,
554}
555
556/// Error from [`solve_with_policy`].
557#[derive(Debug, Clone)]
558pub enum SolvePolicyError {
559    /// The underlying SPP solver failed.
560    Solve(SppError),
561    /// The solved receiver state failed a business-level validation gate.
562    Validation(SolutionValidationError),
563    /// Coarse search found no converged redundant candidate.
564    NoCoarseSolution,
565}
566
567impl From<SppError> for SolvePolicyError {
568    fn from(error: SppError) -> Self {
569        Self::Solve(error)
570    }
571}
572
573impl From<SolutionValidationError> for SolvePolicyError {
574    fn from(error: SolutionValidationError) -> Self {
575        Self::Validation(error)
576    }
577}
578
579impl core::fmt::Display for SolvePolicyError {
580    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
581        match self {
582            Self::Solve(error) => write!(f, "SPP solve failed: {error}"),
583            Self::Validation(error) => write!(f, "SPP validation failed: {error}"),
584            Self::NoCoarseSolution => write!(f, "coarse search found no converged SPP solution"),
585        }
586    }
587}
588
589impl std::error::Error for SolvePolicyError {
590    fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
591        match self {
592            Self::Solve(error) => Some(error),
593            Self::Validation(error) => Some(error),
594            Self::NoCoarseSolution => None,
595        }
596    }
597}
598
599/// The SPP measurement-model operation-order selections, resolved from a
600/// strategy's [`EstimationRecipe`]: the transmit-time light-time range recipe,
601/// the Sagnac rotation recipe, and the receiver-frame (geodetic / az-el) recipe.
602///
603/// Threading these into [`sat_model`] is what makes SPP consume its
604/// `recipe.range` / `recipe.sagnac` / `recipe.frame` rather than hard-coding a
605/// single op-order. [`Self::reference`] is the SPP Skyfield reference selection,
606/// so the legacy entry points reproduce the current behavior bit-for-bit.
607#[derive(Debug, Clone, Copy, PartialEq, Eq)]
608pub(crate) struct SppModelRecipe {
609    pub range: RangeRecipe,
610    pub sagnac: SagnacRecipe,
611    pub frame: FrameRecipe,
612}
613
614impl SppModelRecipe {
615    /// The model selections carried by `recipe` (its range/sagnac/frame stages).
616    pub(crate) const fn from_recipe(recipe: &EstimationRecipe) -> Self {
617        Self {
618            range: recipe.range,
619            sagnac: recipe.sagnac,
620            frame: recipe.frame,
621        }
622    }
623
624    /// The SPP Skyfield reference model selections (the
625    /// [`EstimationRecipe::spp`] range/sagnac/frame stages).
626    pub(crate) const fn reference() -> Self {
627        Self::from_recipe(&EstimationRecipe::spp())
628    }
629}
630
631/// Per-satellite model used by the solve path: the Sagnac-rotated satellite
632/// position, the topocentric az/el, and the predicted pseudorange.
633///
634/// In test builds the struct additionally carries the named intermediate
635/// quantities (transmit time, satellite ECEF, Sagnac angle, geometric range,
636/// ionosphere, troposphere) so the 0-ULP trace-replay parity test can assert
637/// each one bit-for-bit against the reference recipe; the solve path never
638/// reads them, so they are gated out of production builds.
639#[derive(Debug, Clone, Copy)]
640pub(crate) struct SatModel {
641    pub sat_rot_ecef_m: [f64; 3],
642    pub el_rad: f64,
643    pub p_hat_m: f64,
644    #[cfg(all(test, sidereon_repo_tests))]
645    pub az_rad: f64,
646    #[cfg(all(test, sidereon_repo_tests))]
647    pub tau_s: f64,
648    #[cfg(all(test, sidereon_repo_tests))]
649    pub t_tx_j2000_s: f64,
650    #[cfg(all(test, sidereon_repo_tests))]
651    pub sat_ecef_m: [f64; 3],
652    #[cfg(all(test, sidereon_repo_tests))]
653    pub dt_sat_s: f64,
654    #[cfg(all(test, sidereon_repo_tests))]
655    pub theta_rad: f64,
656    #[cfg(all(test, sidereon_repo_tests))]
657    pub rho_m: f64,
658    #[cfg(all(test, sidereon_repo_tests))]
659    pub iono_m: f64,
660    #[cfg(all(test, sidereon_repo_tests))]
661    pub tropo_m: f64,
662}
663
664/// The broadcast ionosphere correction a satellite's system uses.
665#[derive(Debug, Clone, Copy, PartialEq)]
666pub(crate) enum SppIonosphere {
667    /// GPS/BeiDou Klobuchar alpha/beta model.
668    Klobuchar(KlobucharCoeffs),
669    /// Galileo NeQuick-G effective-ionisation coefficients.
670    GalileoNequick(GalileoNequickCoeffs),
671}
672
673/// The ionosphere coefficients a satellite's system uses: Galileo prefers its
674/// `galileo_nequick` (`GAL`) set when present; BeiDou prefers its
675/// `beidou_klobuchar` (`BDSA`/`BDSB`) set when present; all missing
676/// constellation-specific sets fall back to the shared GPS Klobuchar values to
677/// preserve existing callers.
678fn ionosphere_for(system: GnssSystem, inputs: &SolveInputs) -> SppIonosphere {
679    match (system, inputs.galileo_nequick, inputs.beidou_klobuchar) {
680        (GnssSystem::Galileo, Some(gal), _) => SppIonosphere::GalileoNequick(gal),
681        (GnssSystem::BeiDou, _, Some(bds)) => SppIonosphere::Klobuchar(bds),
682        _ => SppIonosphere::Klobuchar(inputs.klobuchar),
683    }
684}
685
686/// Per-epoch inputs shared by every satellite's [`sat_model`] evaluation in a
687/// solve: the ephemeris source plus the epoch and correction arguments that do
688/// not vary between satellites. Bundling them lets [`sat_model`] take only the
689/// per-satellite arguments (id, receiver state, measurement, system Klobuchar)
690/// instead of a long positional parameter list.
691pub(crate) struct SatModelEnv<'a> {
692    pub eph: &'a dyn EphemerisSource,
693    /// Receive epoch, seconds since J2000 in the SP3 product's time scale.
694    pub t_rx_j2000_s: f64,
695    /// GPS second-of-day of the receive epoch (Klobuchar diurnal argument).
696    pub t_rx_second_of_day_s: f64,
697    /// Fractional day-of-year of the receive epoch (Niell seasonal argument).
698    pub day_of_year: f64,
699    /// The correction terms to apply.
700    pub corrections: Corrections,
701    /// Surface meteorology (used iff `corrections.troposphere`).
702    pub met: &'a SurfaceMet,
703    /// GLONASS FDMA channel numbers keyed by slot (PRN), used to resolve the
704    /// per-satellite GLONASS carrier for the ionosphere scaling.
705    pub glonass_channels: &'a BTreeMap<u8, i8>,
706    /// The range/sagnac/frame operation-order selections [`sat_model`] consumes,
707    /// resolved from the strategy's recipe.
708    pub model: SppModelRecipe,
709}
710
711/// Build the per-satellite predicted pseudorange in the SPP operation order
712/// SELECTED BY THE RECIPE on [`SatModelEnv::model`], sharing the
713/// parity-sensitive range and frame substrate with the other strategies.
714///
715/// The three model stages are read from the recipe rather than hard-coded:
716/// - **range** (`env.model.range`): the transmit-time light-time iteration.
717///   [`RangeRecipe::SppMeasuredPseudorangeFixedIter`] (the SPP reference) seeds
718///   `tau` from the measured pseudorange and runs a fixed iteration count (no
719///   convergence test). [`RangeRecipe::CanonicalLightTimeClosedFormSagnac`] (the
720///   canonical strategy) seeds the same way but iterates the light-time loop to
721///   convergence (the IERS-rigorous op-order). These are the two light-time
722///   recipes the SPP measurement model implements; the observable
723///   rounded-microsecond and RTK provided-transmit recipes are other strategies'
724///   range models and never reach here.
725/// - **sagnac** (`env.model.sagnac`): the closed-form Sagnac Z-rotation and the
726///   pre/post-rotation geometric range route through
727///   [`crate::estimation::substrate::range`] under the selected recipe.
728/// - **frame** (`env.model.frame`): the receiver geodetic conversion and the
729///   geodetic ENU azimuth/elevation route through
730///   [`crate::estimation::substrate::frames`] under the selected recipe (the SPP
731///   reference selects [`FrameRecipe::SppSkyfieldAuThreeIter`], the Skyfield AU
732///   three-iteration solve).
733///
734/// The raw residual ([`residual_unweighted`], `P_meas - P_hat`) the trust-region
735/// finite-difference solver differences carries no design rows of its own; the
736/// substrate [`crate::estimation::substrate::rows`] `ResidualRow` assembly serves
737/// the RTK/PPP normal-equation stacks.
738///
739/// Returns `None` if the ephemeris source has no usable position/clock for the
740/// satellite at the transmit epoch.
741pub(crate) fn sat_model(
742    env: &SatModelEnv,
743    sat: GnssSatelliteId,
744    rx_ecef_m: [f64; 3],
745    b_m: f64,
746    p_meas_m: f64,
747    ionosphere: SppIonosphere,
748) -> Option<SatModel> {
749    let sagnac = env.model.sagnac;
750    let frame = env.model.frame;
751
752    // Transmit-time light-time iteration, selected by the range recipe.
753    let (sat_pos, dt_sat, tau) = match env.model.range {
754        RangeRecipe::SppMeasuredPseudorangeFixedIter => {
755            // Fixed iteration count, no inner convergence test; seed tau from the
756            // measured pseudorange.
757            let mut tau = p_meas_m / C_M_S;
758            let mut t_tx = env.t_rx_j2000_s - tau;
759            let mut sat_pos = [0.0f64; 3];
760            let mut dt_sat = 0.0f64;
761            for _ in 0..TRANSMIT_TIME_ITERATIONS {
762                let (pos, clk) = env.eph.position_clock_at_j2000_s(sat, t_tx)?;
763                sat_pos = pos;
764                dt_sat = clk;
765                // Pre-rotation geometric range through the shared substrate (the
766                // closed-form recipe = plain `norm3(sub3(sat, recv))`).
767                let rho0 = geometric_range(sagnac, sat_pos, rx_ecef_m, OMEGA_E_DOT_RAD_S, C_M_S);
768                tau = rho0 / C_M_S;
769                t_tx = env.t_rx_j2000_s - tau;
770            }
771            (sat_pos, dt_sat, tau)
772        }
773        RangeRecipe::CanonicalLightTimeClosedFormSagnac => {
774            // Full iterative light-time (the IERS-rigorous op-order): iterate the
775            // transmit epoch until the signal travel time stops changing, rather
776            // than the reference recipe's fixed two-iteration truncation. Seeded,
777            // like the reference, from the measured pseudorange; the iteration
778            // converges to the geometric light-time fixed point
779            // `t_tx = t_rx - rho(t_tx)/c` with the closed-form Sagnac range (never
780            // a first-order scalar Sagnac). The satellite clock `dt_sat` returned
781            // by the ephemeris already carries the relativistic periodic term
782            // (the broadcast Keplerian evaluation applies `F*e*sqrt(A)*sin(E)`;
783            // SP3 precise clocks include it, the SPP L3 no-op), so the canonical
784            // relativistically-correct range consumes it directly with no
785            // double-counting term.
786            let mut tau = p_meas_m / C_M_S;
787            let mut t_tx = env.t_rx_j2000_s - tau;
788            let mut sat_pos = [0.0f64; 3];
789            let mut dt_sat = 0.0f64;
790            let mut prev_tau = f64::INFINITY;
791            for _ in 0..CANONICAL_LIGHT_TIME_MAX_ITERS {
792                let (pos, clk) = env.eph.position_clock_at_j2000_s(sat, t_tx)?;
793                sat_pos = pos;
794                dt_sat = clk;
795                let rho0 = geometric_range(sagnac, sat_pos, rx_ecef_m, OMEGA_E_DOT_RAD_S, C_M_S);
796                tau = rho0 / C_M_S;
797                t_tx = env.t_rx_j2000_s - tau;
798                if (tau - prev_tau).abs() <= CANONICAL_LIGHT_TIME_TOL_S {
799                    break;
800                }
801                prev_tau = tau;
802            }
803            (sat_pos, dt_sat, tau)
804        }
805        RangeRecipe::ObservableRoundedMicrosecondFixedIter
806        | RangeRecipe::RtkProvidedTxFirstOrderSagnac => unreachable!(
807            "the SPP measurement model runs only the measured-pseudorange or canonical light-time recipe"
808        ),
809    };
810
811    // Sagnac / Earth-rotation rotation over the flight time, selected by recipe.
812    let sat_rot = rotate_transmit_satellite(sagnac, sat_pos, tau, OMEGA_E_DOT_RAD_S);
813
814    // Geometric range (post-Sagnac) through the shared substrate.
815    let rho = geometric_range(sagnac, sat_rot, rx_ecef_m, OMEGA_E_DOT_RAD_S, C_M_S);
816
817    // Geometry for corrections: az/el from rx and the Sagnac-rotated satellite,
818    // through the recipe-selected frame substrate.
819    let g = az_el_from_ecef(frame, rx_ecef_m, sat_rot);
820
821    let mut iono_m = 0.0;
822    let mut tropo_m = 0.0;
823    if env.corrections.ionosphere {
824        // The SPP 0-ULP trace oracle pins this multiply-then-divide order.
825        let lat_deg = g.geodetic.lat_rad * 180.0 / PI;
826        let lon_deg = g.geodetic.lon_rad * 180.0 / PI;
827        let az_deg = g.az_rad * 180.0 / PI;
828        let el_deg = g.el_rad * 180.0 / PI;
829        // A used satellite always has a resolvable carrier here (the solve
830        // rejects an ionosphere request for any satellite that does not, GLONASS
831        // included via its FDMA channel), so the fallback is unreachable. The
832        // GLONASS per-satellite carrier makes the Klobuchar delay scale by
833        // `(f_L1 / f_k)^2` inside the kernel, exactly as RTKLIB-demo5 does.
834        let freq_hz = spp_iono_frequency_hz(sat, env.glonass_channels).unwrap_or(F_L1_HZ);
835        iono_m = match ionosphere {
836            SppIonosphere::Klobuchar(klobuchar) => klobuchar_native_unchecked(
837                &KlobucharParams {
838                    alpha: klobuchar.alpha,
839                    beta: klobuchar.beta,
840                },
841                lat_deg,
842                lon_deg,
843                az_deg,
844                el_deg,
845                env.t_rx_second_of_day_s,
846                freq_hz,
847            ),
848            SppIonosphere::GalileoNequick(coeffs) => galileo_nequick_g_native_unchecked(
849                &coeffs,
850                GalileoNequickEval {
851                    lat_deg,
852                    lon_deg,
853                    el_deg,
854                    t_gal_s: env.t_rx_second_of_day_s,
855                    day_of_year: env.day_of_year,
856                    frequency_hz: freq_hz,
857                },
858            ),
859        };
860    }
861    if env.corrections.troposphere {
862        tropo_m = slant_components(
863            g.el_rad,
864            g.geodetic,
865            env.met.pressure_hpa,
866            env.met.temperature_k,
867            env.met.relative_humidity,
868            env.day_of_year,
869        )
870        .slant_m;
871    }
872
873    // Predicted pseudorange, left-to-right; c*dt_sat is a single multiply.
874    let p_hat = rho + b_m - C_M_S * dt_sat + iono_m + tropo_m;
875
876    Some(SatModel {
877        sat_rot_ecef_m: sat_rot,
878        el_rad: g.el_rad,
879        p_hat_m: p_hat,
880        #[cfg(all(test, sidereon_repo_tests))]
881        az_rad: g.az_rad,
882        #[cfg(all(test, sidereon_repo_tests))]
883        tau_s: tau,
884        // Bit-identical to the loop's final `t_tx = t_rx - tau` (same operands).
885        #[cfg(all(test, sidereon_repo_tests))]
886        t_tx_j2000_s: env.t_rx_j2000_s - tau,
887        #[cfg(all(test, sidereon_repo_tests))]
888        sat_ecef_m: sat_pos,
889        #[cfg(all(test, sidereon_repo_tests))]
890        dt_sat_s: dt_sat,
891        #[cfg(all(test, sidereon_repo_tests))]
892        theta_rad: OMEGA_E_DOT_RAD_S * tau,
893        #[cfg(all(test, sidereon_repo_tests))]
894        rho_m: rho,
895        #[cfg(all(test, sidereon_repo_tests))]
896        iono_m,
897        #[cfg(all(test, sidereon_repo_tests))]
898        tropo_m,
899    })
900}
901
902/// The frozen-geometry selection: used satellites (ascending id), rejected
903/// satellites with reason, and the per-used-sat weight from the elevation at
904/// the initial-guess geometry.
905pub(crate) struct Selection {
906    pub used: Vec<GnssSatelliteId>,
907    pub rejected: Vec<RejectedSat>,
908    /// `weight` per used satellite, index-aligned to `used`.
909    pub weights: Vec<f64>,
910}
911
912pub(crate) fn select_sats(
913    eph: &dyn EphemerisSource,
914    inputs: &SolveInputs,
915    model: SppModelRecipe,
916) -> Selection {
917    let rx0 = [
918        inputs.initial_guess[0],
919        inputs.initial_guess[1],
920        inputs.initial_guess[2],
921    ];
922    let b0 = inputs.initial_guess[3];
923
924    // Ascending satellite-id order, never observation order.
925    let mut obs: Vec<&Observation> = inputs.observations.iter().collect();
926    obs.sort_by_key(|o| o.satellite_id);
927
928    let mut used = Vec::new();
929    let mut rejected = Vec::new();
930    let mut weights = Vec::new();
931
932    let env = SatModelEnv {
933        eph,
934        t_rx_j2000_s: inputs.t_rx_j2000_s,
935        t_rx_second_of_day_s: inputs.t_rx_second_of_day_s,
936        day_of_year: inputs.day_of_year,
937        corrections: inputs.corrections,
938        met: &inputs.met,
939        glonass_channels: &inputs.glonass_channels,
940        model,
941    };
942    for ob in obs {
943        let model = sat_model(
944            &env,
945            ob.satellite_id,
946            rx0,
947            b0,
948            ob.pseudorange_m,
949            ionosphere_for(ob.satellite_id.system, inputs),
950        );
951        let model = match model {
952            Some(m) => m,
953            None => {
954                rejected.push(RejectedSat {
955                    satellite_id: ob.satellite_id,
956                    reason: RejectionReason::NoEphemeris,
957                });
958                continue;
959            }
960        };
961        if model.el_rad < ELEVATION_MASK_RAD {
962            rejected.push(RejectedSat {
963                satellite_id: ob.satellite_id,
964                reason: RejectionReason::LowElevation,
965            });
966            continue;
967        }
968        let sin_el = model.el_rad.sin();
969        let weight = (sin_el * sin_el) / (SIGMA0_M * SIGMA0_M);
970        used.push(ob.satellite_id);
971        weights.push(weight);
972    }
973
974    Selection {
975        used,
976        rejected,
977        weights,
978    }
979}
980
981/// The distinct GNSS present in `used`, in ascending system order.
982///
983/// The receiver-clock part of the state has one entry per system, each the
984/// *absolute* receiver clock for that system (not a bias); the first is the
985/// reference clock and a system's inter-system bias is its clock minus that
986/// reference. For a single-system solve this is one element and the state is the
987/// classic `[x, y, z, b]`.
988pub(crate) fn clock_systems(used: &[GnssSatelliteId]) -> Vec<GnssSystem> {
989    let mut systems: Vec<GnssSystem> = used.iter().map(|s| s.system).collect();
990    systems.sort_unstable();
991    systems.dedup();
992    systems
993}
994
995/// The unweighted residual vector `P_meas - P_hat` at state `x`, in `used` order.
996///
997/// The state is `[x, y, z, clk_0, clk_1, ...]` where `clk_i` is the absolute
998/// receiver clock for the i-th system returned by [`clock_systems`] (in meters).
999/// Each satellite's residual uses its own system's clock, so a multi-GNSS set is
1000/// solved with one absolute receiver clock per system (a system's inter-system
1001/// bias is its clock minus the reference `clk_0`). A single-system set reduces to
1002/// `[x, y, z, b]` and `clk_0 = x[3]`.
1003///
1004/// Returns `Err(satellite)` if a used satellite has no observation or no usable
1005/// ephemeris at `x` (the frozen used set is fixed, but a finite-difference probe
1006/// could in principle reach an epoch off the ephemeris coverage). The caller
1007/// turns that into an [`SppError`] rather than panicking.
1008pub(crate) fn residual_unweighted(
1009    eph: &dyn EphemerisSource,
1010    used: &[GnssSatelliteId],
1011    obs_by_id: &[(GnssSatelliteId, f64)],
1012    x: &[f64],
1013    inputs: &SolveInputs,
1014    model: SppModelRecipe,
1015) -> Result<Vec<f64>, GnssSatelliteId> {
1016    let rx = [x[0], x[1], x[2]];
1017    let systems = clock_systems(used);
1018    let env = SatModelEnv {
1019        eph,
1020        t_rx_j2000_s: inputs.t_rx_j2000_s,
1021        t_rx_second_of_day_s: inputs.t_rx_second_of_day_s,
1022        day_of_year: inputs.day_of_year,
1023        corrections: inputs.corrections,
1024        met: &inputs.met,
1025        glonass_channels: &inputs.glonass_channels,
1026        model,
1027    };
1028    let mut out = Vec::with_capacity(used.len());
1029    for &sat in used {
1030        let p_meas = obs_by_id
1031            .iter()
1032            .find(|(id, _)| *id == sat)
1033            .map(|(_, p)| *p)
1034            .ok_or(sat)?;
1035        // The clock for this satellite's system (index 0 = reference clock).
1036        let sys_idx = systems.iter().position(|s| *s == sat.system).unwrap_or(0);
1037        let b = x[3 + sys_idx];
1038        let m =
1039            sat_model(&env, sat, rx, b, p_meas, ionosphere_for(sat.system, inputs)).ok_or(sat)?;
1040        out.push(p_meas - m.p_hat_m);
1041    }
1042    Ok(out)
1043}
1044
1045/// Run the SPP solve from synthesized/measured pseudoranges.
1046///
1047/// Uses the core trust-region weighted least-squares solver over the
1048/// `sqrt(w) * (P_meas - P_hat)` residual. The converged position/clock is a
1049/// sub-micron solver-agreement result (the linear-algebra step is not
1050/// bit-reproducible across BLAS builds), not a 0-ULP claim. The residual /
1051/// Jacobian substrate evaluated at recorded states is the 0-ULP target and is
1052/// exercised by the trace-replay parity test, not by this entry point.
1053///
1054/// This is the reference SPP entry point: it runs the legacy
1055/// [`SolverRecipe::NalgebraTrfLegacy`] trust-region factorization, so its
1056/// existing goldens are unchanged. [`solve_with_solver`] selects the owned
1057/// deterministic kernel.
1058pub fn solve(
1059    eph: &dyn EphemerisSource,
1060    inputs: &SolveInputs,
1061    with_geodetic: bool,
1062) -> Result<ReceiverSolution, SppError> {
1063    validate_solve_inputs(inputs)?;
1064    solve_inner(
1065        eph,
1066        inputs,
1067        with_geodetic,
1068        SppModelRecipe::reference(),
1069        TrustRegionSolve::NalgebraLu,
1070    )
1071}
1072
1073/// SPP's trust-region stage recognizes the owned deterministic solver
1074/// ([`SolverRecipe::OwnedDeterministicTrf`]), which owns the dense subproblem
1075/// factorization with a fixed reduction order and its own frozen-bits golden;
1076/// every other recipe selects the legacy nalgebra LU path that [`solve`] uses.
1077/// The other [`SolverRecipe`] variants name other strategies' linear-solve
1078/// stages (RTK first-tie, PPP last-tie, host LAPACK) and are not SPP
1079/// trust-region solvers.
1080const fn trust_region_solve(solver: SolverRecipe) -> TrustRegionSolve {
1081    match solver {
1082        SolverRecipe::OwnedDeterministicTrf => TrustRegionSolve::OwnedGaussianFirstTie,
1083        _ => TrustRegionSolve::NalgebraLu,
1084    }
1085}
1086
1087/// SPP solve with an explicit [`SolverRecipe`] for the trust-region stage.
1088///
1089/// Selecting [`SolverRecipe::NalgebraTrfLegacy`] is bit-identical to [`solve`].
1090/// [`SolverRecipe::OwnedDeterministicTrf`] swaps in the owned deterministic
1091/// Gaussian-elimination factorization for the dense trust-region subproblem (no
1092/// nalgebra LU, no black-box BLAS in that solve), pinned to its own frozen-bits
1093/// golden; all other model stages are unchanged. The owned kernel owns ONLY the
1094/// subproblem factorization: the normal-matrix / gradient / norm reductions that
1095/// build the subproblem still go through nalgebra's CPU-dispatched dense
1096/// algebra, so the cross-platform bit guarantee is scoped to the factorization
1097/// (the converged bits are this build's reproducible output, not a portable
1098/// constant).
1099pub fn solve_with_solver(
1100    eph: &dyn EphemerisSource,
1101    inputs: &SolveInputs,
1102    with_geodetic: bool,
1103    solver: SolverRecipe,
1104) -> Result<ReceiverSolution, SppError> {
1105    validate_solve_inputs(inputs)?;
1106    solve_inner(
1107        eph,
1108        inputs,
1109        with_geodetic,
1110        SppModelRecipe::reference(),
1111        trust_region_solve(solver),
1112    )
1113}
1114
1115fn solve_inner(
1116    eph: &dyn EphemerisSource,
1117    inputs: &SolveInputs,
1118    with_geodetic: bool,
1119    model: SppModelRecipe,
1120    linear_solve: TrustRegionSolve,
1121) -> Result<ReceiverSolution, SppError> {
1122    // One pseudorange per satellite. Reject duplicates deterministically (by
1123    // the smallest repeated id) so the result can never depend on observation
1124    // order and the >= 4 check below counts distinct satellites.
1125    let mut ids: Vec<GnssSatelliteId> =
1126        inputs.observations.iter().map(|o| o.satellite_id).collect();
1127    ids.sort_unstable();
1128    if let Some(w) = ids.windows(2).find(|w| w[0] == w[1]) {
1129        return Err(SppError::DuplicateObservation { satellite: w[0] });
1130    }
1131
1132    // The broadcast Klobuchar delay is computed on L1 and scaled to each
1133    // satellite's carrier by `(f_L1 / f)^2`. GPS L1, Galileo E1, and BeiDou B1I
1134    // have fixed carriers; GLONASS is FDMA, so its carrier is resolved per
1135    // satellite from `glonass_channels`. A satellite whose carrier cannot be
1136    // resolved (a GLONASS observation with no channel in the map, or a channel
1137    // outside the valid `[-7, +6]` FDMA range) cannot be scaled, so reject an
1138    // ionosphere-corrected solve that includes it rather
1139    // than apply an undefined correction. This runs before selection so the
1140    // model is never evaluated for it (`select_sats` would otherwise call
1141    // `sat_model` with the correction for every observation).
1142    if inputs.corrections.ionosphere {
1143        if let Some(sat) = ids
1144            .iter()
1145            .find(|s| spp_iono_frequency_hz(**s, &inputs.glonass_channels).is_none())
1146        {
1147            return Err(SppError::IonosphereUnsupported { satellite: *sat });
1148        }
1149    }
1150
1151    let sel = select_sats(eph, inputs, model);
1152
1153    // One receiver-clock parameter per distinct GNSS (a reference clock plus an
1154    // inter-system bias for each additional system), so the state has
1155    // `3 + n_systems` parameters and needs at least that many usable satellites.
1156    // Floor the clock count at one: the minimum solve is the four-parameter
1157    // single-system form even when no satellite survives selection.
1158    let systems = clock_systems(&sel.used);
1159    let n_clocks = systems.len();
1160    // SPP's weighted-residual rows feed the trust-region solver, which owns the
1161    // normal-equation factorization (NormalRecipe::SppWeightedResidualFiniteDifference
1162    // via SolverRecipe::NalgebraTrfLegacy); only the parameter stack is named here.
1163    let n_params = ParameterLayout::spp(n_clocks.max(1)).dim();
1164    if sel.used.len() < n_params {
1165        return Err(SppError::TooFewSatellites {
1166            used: sel.used.len(),
1167            required: n_params,
1168        });
1169    }
1170
1171    let obs_by_id: Vec<(GnssSatelliteId, f64)> = inputs
1172        .observations
1173        .iter()
1174        .map(|o| (o.satellite_id, o.pseudorange_m))
1175        .collect();
1176
1177    let used = sel.used.clone();
1178    let inputs_ref = inputs.clone();
1179    let obs_ref = obs_by_id.clone();
1180    let eph_ref = eph;
1181    let n_used = used.len();
1182
1183    // The least-squares solver's residual closure cannot return an error, so an
1184    // ephemeris loss during a probe is recorded here and surfaced as an
1185    // SppError after the solve (rather than panicking inside the closure).
1186    let lost = std::rc::Rc::new(std::cell::Cell::new(None::<GnssSatelliteId>));
1187    let lost_in = lost.clone();
1188    let residual = move |x: &DVector<f64>| -> DVector<f64> {
1189        match residual_unweighted(eph_ref, &used, &obs_ref, x.as_slice(), &inputs_ref, model) {
1190            Ok(r) => DVector::from_vec(r),
1191            Err(sat) => {
1192                lost_in.set(Some(sat));
1193                DVector::from_vec(vec![0.0; n_used])
1194            }
1195        }
1196    };
1197
1198    // Extend the 4-element initial guess `[x, y, z, b_ref]` with a zero starting
1199    // value for each additional system's inter-system bias.
1200    let mut x0v = inputs.initial_guess.to_vec();
1201    x0v.extend(std::iter::repeat_n(0.0, n_clocks - 1));
1202    let x0 = DVector::from_vec(x0v);
1203    // Agreement-track stopping thresholds (see the SPP_SOLVER_* constants).
1204    let opts = SolveOptions {
1205        gtol: SPP_SOLVER_GTOL,
1206        ftol: SPP_SOLVER_FTOL,
1207        xtol: SPP_SOLVER_XTOL,
1208        max_nfev: SPP_SOLVER_MAX_NFEV,
1209    };
1210
1211    // The static elevation weights (base weights), index-aligned to `sel.used`.
1212    let base_weights = DVector::from_row_slice(&sel.weights);
1213
1214    // The warm-start solve uses the base elevation weights exactly. On the
1215    // static path (`robust == None`) this is the literal current sequence: a
1216    // single `with_weights(residual, x0, base_weights)` solve and nothing else,
1217    // so the byte output is unchanged. On the robust path it seeds the outer
1218    // loop.
1219    //
1220    // Check for an ephemeris loss recorded by the residual closure BEFORE
1221    // propagating a solver error: a lost satellite zeroes its residual row,
1222    // which can itself make the Jacobian singular, and EphemerisLost is the
1223    // more specific, actionable cause.
1224    let problem = LeastSquaresProblem::with_weights(&residual, x0.clone(), base_weights.clone());
1225    let report_result = solve_trf_with(&problem, &opts, linear_solve);
1226    if let Some(satellite) = lost.get() {
1227        return Err(SppError::EphemerisLost { satellite });
1228    }
1229    let mut report = report_result?;
1230
1231    let mut outer_iterations = 0usize;
1232    let mut final_robust_scale_m: Option<f64> = None;
1233
1234    // Outer Huber/IRLS reweighting loop, ONLY on the robust path. Each iteration
1235    // recomputes the unweighted post-fit residuals at the current converged
1236    // state, derives a floored MAD scale, builds the effective weight vector
1237    // `base_elevation_weight * huber(r_i / s)` index-aligned to `sel.used`,
1238    // rebuilds the problem warm-started at the previous state, and re-solves. It
1239    // stops when the position step drops below `outer_tol_m` or the reweighted
1240    // solve budget left after the warm start is hit (recording
1241    // `converged = false` if the inner solve itself did not converge on the
1242    // final pass).
1243    if let Some(rc) = inputs.robust {
1244        for _ in 0..rc.max_outer.saturating_sub(1) {
1245            if lost.get().is_some() {
1246                break;
1247            }
1248            // Unweighted post-fit residuals at the current state, in used order.
1249            let post = match residual_unweighted(
1250                eph,
1251                &sel.used,
1252                &obs_by_id,
1253                report.x.as_slice(),
1254                inputs,
1255                model,
1256            ) {
1257                Ok(r) => r,
1258                Err(satellite) => return Err(SppError::EphemerisLost { satellite }),
1259            };
1260            let scale = mad_scale(&post, rc.scale_floor_m).map_err(map_robust_error)?;
1261            // Effective weight per used sat: base elevation weight times the
1262            // Huber multiplier of the scaled residual.
1263            let eff: Vec<f64> = post
1264                .iter()
1265                .zip(sel.weights.iter())
1266                .map(|(&r, &bw)| bw * huber_weight(r / scale, rc.huber_k))
1267                .collect();
1268            let eff_w = DVector::from_row_slice(&eff);
1269            let x_prev = report.x.clone();
1270            let problem = LeastSquaresProblem::with_weights(&residual, x_prev.clone(), eff_w);
1271            let next = solve_trf_with(&problem, &opts, linear_solve);
1272            if let Some(satellite) = lost.get() {
1273                return Err(SppError::EphemerisLost { satellite });
1274            }
1275            report = next?;
1276            outer_iterations += 1;
1277            final_robust_scale_m = Some(scale);
1278            // Position L2 step between successive outer solves.
1279            let dpos = ((report.x[0] - x_prev[0]).powi(2)
1280                + (report.x[1] - x_prev[1]).powi(2)
1281                + (report.x[2] - x_prev[2]).powi(2))
1282            .sqrt();
1283            if dpos < rc.outer_tol_m {
1284                break;
1285            }
1286        }
1287    }
1288
1289    let xs = &report.x;
1290    let position = ItrfPositionM::new(xs[0], xs[1], xs[2]).expect("valid ITRF position");
1291    let rx_clock_s = xs[3] / C_M_S;
1292    // One receiver clock (seconds) per system, in the same order as the state's
1293    // clock parameters. The first equals `rx_clock_s` (the reference system).
1294    let system_clocks_s: Vec<(GnssSystem, f64)> = systems
1295        .iter()
1296        .enumerate()
1297        .map(|(i, &sys)| (sys, xs[3 + i] / C_M_S))
1298        .collect();
1299    let geodetic = if with_geodetic {
1300        Some(geodetic_from_ecef(model.frame, [xs[0], xs[1], xs[2]]))
1301    } else {
1302        None
1303    };
1304
1305    // Post-fit unweighted residuals in used order.
1306    let residuals_m = residual_unweighted(eph, &sel.used, &obs_by_id, xs.as_slice(), inputs, model)
1307        .map_err(|satellite| SppError::EphemerisLost { satellite })?;
1308
1309    // DOP from the converged geometry: line-of-sight unit vectors to the
1310    // Sagnac-rotated satellite positions, with the frozen weights. A
1311    // single-system solve uses the 0-ULP four-state cofactor inverse; a
1312    // multi-system solve uses the general (3 + n_systems) inverse with one clock
1313    // column per GNSS (a deterministic geometry diagnostic, not a 0-ULP target).
1314    // The receiver-clock argument does not affect the line of sight, so the
1315    // reference clock is passed for every satellite.
1316    let rx_ecef = [xs[0], xs[1], xs[2]];
1317    let geo = geodetic_from_ecef(model.frame, [xs[0], xs[1], xs[2]]);
1318    let mut los = Vec::with_capacity(sel.used.len());
1319    let mut clock_index = Vec::with_capacity(sel.used.len());
1320    let env = SatModelEnv {
1321        eph,
1322        t_rx_j2000_s: inputs.t_rx_j2000_s,
1323        t_rx_second_of_day_s: inputs.t_rx_second_of_day_s,
1324        day_of_year: inputs.day_of_year,
1325        corrections: inputs.corrections,
1326        met: &inputs.met,
1327        glonass_channels: &inputs.glonass_channels,
1328        model,
1329    };
1330    for &sat in &sel.used {
1331        let p_meas = obs_by_id
1332            .iter()
1333            .find(|(id, _)| *id == sat)
1334            .map(|(_, p)| *p)
1335            .ok_or(SppError::EphemerisLost { satellite: sat })?;
1336        let m = sat_model(
1337            &env,
1338            sat,
1339            rx_ecef,
1340            xs[3],
1341            p_meas,
1342            ionosphere_for(sat.system, inputs),
1343        )
1344        .ok_or(SppError::EphemerisLost { satellite: sat })?;
1345        let dx = m.sat_rot_ecef_m[0] - rx_ecef[0];
1346        let dy = m.sat_rot_ecef_m[1] - rx_ecef[1];
1347        let dz = m.sat_rot_ecef_m[2] - rx_ecef[2];
1348        let n = (dx * dx + dy * dy + dz * dz).sqrt();
1349        los.push(LineOfSight::new(dx / n, dy / n, dz / n));
1350        let idx = systems.iter().position(|s| *s == sat.system).unwrap_or(0);
1351        clock_index.push(idx);
1352    }
1353    let dop_result = if n_clocks == 1 {
1354        dop(&los, &sel.weights, geo).ok()
1355    } else {
1356        dop_multi(&los, &clock_index, n_clocks, &sel.weights, geo).ok()
1357    };
1358
1359    let converged = matches!(
1360        report.status,
1361        Status::GradientTolerance | Status::CostTolerance | Status::StepTolerance
1362    );
1363    let metadata_systems = systems.clone();
1364    let metadata_used_count = sel.used.len();
1365    let metadata_redundancy = redundancy(&metadata_systems, metadata_used_count);
1366
1367    Ok(ReceiverSolution {
1368        position,
1369        geodetic,
1370        rx_clock_s,
1371        system_clocks_s,
1372        dop: dop_result,
1373        residuals_m,
1374        used_sats: sel.used,
1375        rejected_sats: sel.rejected,
1376        metadata: SolutionMetadata {
1377            iterations: report.iterations,
1378            converged,
1379            status: report.status,
1380            ionosphere_applied: inputs.corrections.ionosphere,
1381            troposphere_applied: inputs.corrections.troposphere,
1382            outer_iterations,
1383            final_robust_scale_m,
1384            used_count: metadata_used_count,
1385            systems: metadata_systems,
1386            redundancy: metadata_redundancy,
1387            raim_checkable: metadata_redundancy >= 1,
1388        },
1389    })
1390}
1391
1392/// Run SPP under the public API's language-independent validation/orchestration
1393/// policy.
1394///
1395/// Thin compatibility wrapper over the runtime strategy selector
1396/// ([`crate::estimation::strategies::estimate`]): it drives the shared
1397/// per-technique implementation [`run`] under the SPP reference strategy, which
1398/// resolves to the SPP reference recipe. The reference strategy always yields an
1399/// SPP solution or an SPP error, so the result is bit-identical to the recipe
1400/// driving [`run`] directly.
1401pub fn solve_with_policy(
1402    eph: &dyn EphemerisSource,
1403    inputs: &SolveInputs,
1404    with_geodetic: bool,
1405    policy: SolvePolicy,
1406) -> Result<ReceiverSolution, SolvePolicyError> {
1407    use crate::estimation::recipe::StrategyId;
1408    use crate::estimation::strategies::{
1409        estimate, EstimateError, EstimateInput, EstimateOptions, EstimateOutput,
1410    };
1411    match estimate(
1412        EstimateInput::Spp {
1413            eph,
1414            inputs,
1415            with_geodetic,
1416            policy,
1417        },
1418        EstimateOptions::new(StrategyId::spp_reference()),
1419    ) {
1420        Ok(EstimateOutput::Spp(solution)) => Ok(*solution),
1421        Err(EstimateError::Spp(error)) => Err(error),
1422        Ok(_) | Err(_) => {
1423            unreachable!("the SPP reference strategy yields an SPP solution or an SPP error")
1424        }
1425    }
1426}
1427
1428/// Solve a batch of independent SPP epochs against a shared ephemeris, serially.
1429///
1430/// Element `i` of the result is [`solve_with_policy`] applied to `epochs[i]`,
1431/// with the shared `eph`, `with_geodetic`, and `policy` (every epoch is one
1432/// receive instant's [`SolveInputs`]; the receiver's clock and position are
1433/// re-estimated per epoch, so the epochs are independent). The first solve error
1434/// for an epoch becomes that element's `Err`. This is the single-threaded
1435/// reference the parallel [`solve_spp_batch_parallel`] is proven bit-identical
1436/// against.
1437pub fn solve_spp_batch_serial(
1438    eph: &dyn EphemerisSource,
1439    epochs: &[SolveInputs],
1440    with_geodetic: bool,
1441    policy: SolvePolicy,
1442) -> Vec<Result<ReceiverSolution, SolvePolicyError>> {
1443    epochs
1444        .iter()
1445        .map(|inputs| solve_with_policy(eph, inputs, with_geodetic, policy))
1446        .collect()
1447}
1448
1449/// Solve a batch of independent SPP epochs against a shared ephemeris, fanning
1450/// the independent per-epoch solves across a rayon thread pool.
1451///
1452/// Each epoch is solved by the same serial [`solve_with_policy`] kernel and the
1453/// indexed parallel collect preserves input order, so element `i` is
1454/// byte-for-byte identical to element `i` of [`solve_spp_batch_serial`]: the
1455/// epochs share only the immutable `eph`/`policy`, there is no cross-epoch state
1456/// and no reduction, and a single solve is unchanged. The work is embarrassingly
1457/// parallel (epochs are independent), so throughput scales with cores while
1458/// every value stays bit-exact. `eph` must be [`Sync`] to be shared across the
1459/// pool.
1460pub fn solve_spp_batch_parallel(
1461    eph: &(dyn EphemerisSource + Sync),
1462    epochs: &[SolveInputs],
1463    with_geodetic: bool,
1464    policy: SolvePolicy,
1465) -> Vec<Result<ReceiverSolution, SolvePolicyError>> {
1466    use rayon::prelude::*;
1467    epochs
1468        .par_iter()
1469        .map(|inputs| solve_with_policy(eph, inputs, with_geodetic, policy))
1470        .collect()
1471}
1472
1473/// Drive SPP from a resolved [`EstimationRecipe`]: the shared per-technique
1474/// implementation that [`crate::estimation::strategies::estimate`] dispatches to.
1475/// The recipe's range/sagnac/frame stages select the SPP measurement-model
1476/// operation order ([`SppModelRecipe`], threaded into [`sat_model`]) and its
1477/// [`SolverRecipe`] selects the trust-region factorization; the public
1478/// validation/orchestration policy is applied here. For the SPP reference recipe
1479/// every selected order equals the value the legacy [`solve`] path hard-coded, so
1480/// this is bit-identical to it.
1481pub(crate) fn run(
1482    recipe: &EstimationRecipe,
1483    eph: &dyn EphemerisSource,
1484    inputs: &SolveInputs,
1485    with_geodetic: bool,
1486    policy: SolvePolicy,
1487) -> Result<ReceiverSolution, SolvePolicyError> {
1488    validate_solve_inputs(inputs)?;
1489    let model = SppModelRecipe::from_recipe(recipe);
1490    match policy.coarse_search_seeds {
1491        Some(seed_count) => solve_coarse(
1492            eph,
1493            inputs,
1494            with_geodetic,
1495            policy,
1496            seed_count,
1497            model,
1498            recipe.solver,
1499        ),
1500        None => solve_validated(
1501            eph,
1502            inputs,
1503            with_geodetic,
1504            policy.validation,
1505            model,
1506            recipe.solver,
1507        ),
1508    }
1509}
1510
1511fn solve_validated(
1512    eph: &dyn EphemerisSource,
1513    inputs: &SolveInputs,
1514    with_geodetic: bool,
1515    validation: SolutionValidationOptions,
1516    model: SppModelRecipe,
1517    solver: SolverRecipe,
1518) -> Result<ReceiverSolution, SolvePolicyError> {
1519    let solution = solve_inner(
1520        eph,
1521        inputs,
1522        with_geodetic,
1523        model,
1524        trust_region_solve(solver),
1525    )?;
1526    validate_receiver_solution(&solution, validation)?;
1527    Ok(solution)
1528}
1529
1530fn solve_coarse(
1531    eph: &dyn EphemerisSource,
1532    inputs: &SolveInputs,
1533    with_geodetic: bool,
1534    policy: SolvePolicy,
1535    seed_count: usize,
1536    model: SppModelRecipe,
1537    solver: SolverRecipe,
1538) -> Result<ReceiverSolution, SolvePolicyError> {
1539    let mut candidates = Vec::new();
1540    let mut last_error = SolvePolicyError::NoCoarseSolution;
1541
1542    for seed in std::iter::once(inputs.initial_guess).chain(coarse_seeds(seed_count)) {
1543        let mut seeded = inputs.clone();
1544        seeded.initial_guess = seed;
1545        match solve_validated(
1546            eph,
1547            &seeded,
1548            with_geodetic,
1549            policy.validation,
1550            model,
1551            solver,
1552        ) {
1553            Ok(solution) => candidates.push(solution),
1554            Err(error) => last_error = error,
1555        }
1556    }
1557
1558    select_coarse_candidate(&candidates)
1559        .cloned()
1560        .ok_or(last_error)
1561}
1562
1563fn coarse_seeds(n: usize) -> Vec<[f64; 4]> {
1564    let golden = PI * (3.0 - 5.0_f64.sqrt());
1565    (0..n)
1566        .map(|i| {
1567            let z = 1.0 - 2.0 * (i as f64 + 0.5) / n as f64;
1568            let r = (1.0 - z * z).max(0.0).sqrt();
1569            let theta = golden * i as f64;
1570            [
1571                MEAN_EARTH_RADIUS_M * r * theta.cos(),
1572                MEAN_EARTH_RADIUS_M * r * theta.sin(),
1573                MEAN_EARTH_RADIUS_M * z,
1574                0.0,
1575            ]
1576        })
1577        .collect()
1578}
1579
1580fn select_coarse_candidate(candidates: &[ReceiverSolution]) -> Option<&ReceiverSolution> {
1581    candidates
1582        .iter()
1583        .filter(|solution| solution.metadata.converged && solution.metadata.redundancy >= 1)
1584        .min_by(|a, b| compare_coarse_candidates(a, b))
1585}
1586
1587fn compare_coarse_candidates(a: &ReceiverSolution, b: &ReceiverSolution) -> core::cmp::Ordering {
1588    b.used_sats
1589        .len()
1590        .cmp(&a.used_sats.len())
1591        .then_with(|| residual_rms(&a.residuals_m).total_cmp(&residual_rms(&b.residuals_m)))
1592        .then_with(|| candidate_gdop(a).total_cmp(&candidate_gdop(b)))
1593}
1594
1595fn candidate_gdop(solution: &ReceiverSolution) -> f64 {
1596    solution.dop.map(|dop| dop.gdop).unwrap_or(f64::INFINITY)
1597}
1598
1599fn residual_rms(residuals: &[f64]) -> f64 {
1600    if residuals.is_empty() {
1601        return 0.0;
1602    }
1603    let sum_sq = residuals.iter().map(|r| r * r).sum::<f64>();
1604    (sum_sq / residuals.len() as f64).sqrt()
1605}
1606
1607fn redundancy(systems: &[GnssSystem], used_count: usize) -> isize {
1608    used_count as isize - (3 + systems.len() as isize)
1609}
1610
1611fn validate_solve_inputs(inputs: &SolveInputs) -> Result<(), SppError> {
1612    validate::finite(inputs.t_rx_j2000_s, "t_rx_j2000_s").map_err(map_input_error)?;
1613    validate::second_of_day(inputs.t_rx_second_of_day_s, "t_rx_second_of_day_s")
1614        .map_err(map_input_error)?;
1615    validate::finite_in_range_exclusive_upper(inputs.day_of_year, 1.0, 367.0, "day_of_year")
1616        .map_err(map_input_error)?;
1617    validate::finite_slice(&inputs.initial_guess, "initial_guess").map_err(map_input_error)?;
1618    validate_klobuchar(&inputs.klobuchar, "klobuchar")?;
1619    if let Some(klobuchar) = &inputs.beidou_klobuchar {
1620        validate_klobuchar(klobuchar, "beidou_klobuchar")?;
1621    }
1622    if let Some(nequick) = &inputs.galileo_nequick {
1623        validate_galileo_nequick(nequick)?;
1624    }
1625    if inputs.corrections.troposphere {
1626        validate_met(&inputs.met)?;
1627    }
1628    validate_observations(&inputs.observations)?;
1629    if let Some(robust) = inputs.robust {
1630        if robust.max_outer == 0 {
1631            return Err(SppError::InvalidInput {
1632                field: "robust.max_outer",
1633                kind: SppInputErrorKind::NotPositive,
1634            });
1635        }
1636        validate::finite_positive(robust.huber_k, "robust.huber_k").map_err(map_input_error)?;
1637        validate::finite_positive(robust.scale_floor_m, "robust.scale_floor_m")
1638            .map_err(map_input_error)?;
1639        validate::finite_positive(robust.outer_tol_m, "robust.outer_tol_m")
1640            .map_err(map_input_error)?;
1641    }
1642    Ok(())
1643}
1644
1645fn validate_klobuchar(coeffs: &KlobucharCoeffs, field: &'static str) -> Result<(), SppError> {
1646    validate::finite_slice(&coeffs.alpha, field).map_err(map_input_error)?;
1647    validate::finite_slice(&coeffs.beta, field).map_err(map_input_error)
1648}
1649
1650fn validate_galileo_nequick(coeffs: &GalileoNequickCoeffs) -> Result<(), SppError> {
1651    validate::finite(coeffs.ai0, "galileo_nequick").map_err(map_input_error)?;
1652    validate::finite(coeffs.ai1, "galileo_nequick").map_err(map_input_error)?;
1653    validate::finite(coeffs.ai2, "galileo_nequick").map_err(map_input_error)?;
1654    Ok(())
1655}
1656
1657fn validate_met(met: &SurfaceMet) -> Result<(), SppError> {
1658    validate::finite_positive(met.pressure_hpa, "met.pressure_hpa").map_err(map_input_error)?;
1659    validate::finite_positive(met.temperature_k, "met.temperature_k").map_err(map_input_error)?;
1660    validate::fraction(met.relative_humidity, "met.relative_humidity").map_err(map_input_error)?;
1661    Ok(())
1662}
1663
1664fn validate_observations(observations: &[Observation]) -> Result<(), SppError> {
1665    for obs in observations {
1666        validate::finite_positive(obs.pseudorange_m, "observation.pseudorange_m")
1667            .map_err(map_input_error)?;
1668    }
1669    Ok(())
1670}
1671
1672fn map_input_error(error: validate::FieldError) -> SppError {
1673    SppError::InvalidInput {
1674        field: error.field(),
1675        kind: SppInputErrorKind::from(&error),
1676    }
1677}
1678
1679fn map_robust_error(error: RobustError) -> SppError {
1680    let field = match error.field() {
1681        "scale_floor" => "robust.scale_floor_m",
1682        "residuals" | "values" => "robust.residuals",
1683        other => other,
1684    };
1685    let kind = match error.reason() {
1686        "not finite" => SppInputErrorKind::NonFinite,
1687        "not positive" => SppInputErrorKind::NotPositive,
1688        "negative" => SppInputErrorKind::Negative,
1689        "out of range" => SppInputErrorKind::OutOfRange,
1690        _ => SppInputErrorKind::OutOfRange,
1691    };
1692    SppError::InvalidInput { field, kind }
1693}
1694
1695/// The core km/deg geodetic recipe, for the boundary cross-check against the
1696/// meters-native helper.
1697#[cfg(all(test, sidereon_repo_tests))]
1698pub(crate) mod test_support {
1699    use super::*;
1700
1701    pub fn geodetic_from_ecef_m_for_test(x_m: f64, y_m: f64, z_m: f64) -> Wgs84Geodetic {
1702        geodetic_from_ecef(FrameRecipe::SppSkyfieldAuThreeIter, [x_m, y_m, z_m])
1703    }
1704
1705    pub fn sat_model_for_test(
1706        env: &SatModelEnv,
1707        sat: GnssSatelliteId,
1708        rx: [f64; 3],
1709        b_m: f64,
1710        p_meas: f64,
1711        klobuchar: &KlobucharCoeffs,
1712    ) -> Option<SatModel> {
1713        sat_model(
1714            env,
1715            sat,
1716            rx,
1717            b_m,
1718            p_meas,
1719            SppIonosphere::Klobuchar(*klobuchar),
1720        )
1721    }
1722
1723    pub fn sat_model_with_ionosphere_for_test(
1724        env: &SatModelEnv,
1725        sat: GnssSatelliteId,
1726        rx: [f64; 3],
1727        b_m: f64,
1728        p_meas: f64,
1729        ionosphere: SppIonosphere,
1730    ) -> Option<SatModel> {
1731        sat_model(env, sat, rx, b_m, p_meas, ionosphere)
1732    }
1733
1734    /// The core km/deg geodetic recipe (Skyfield AU-internal), returning the
1735    /// public `(lat_deg, lon_deg, alt_km)`, for the boundary cross-check.
1736    pub fn itrs_to_geodetic_core_km(x_km: f64, y_km: f64, z_km: f64) -> (f64, f64, f64) {
1737        crate::astro::frames::transforms::itrs_to_geodetic_compute(x_km, y_km, z_km)
1738            .expect("valid ITRS coordinates")
1739    }
1740}
1741
1742#[cfg(all(test, sidereon_repo_tests))]
1743mod tests;