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;