diff --git a/src/solver.rs b/src/solver.rs index bd61c44..da23b40 100644 --- a/src/solver.rs +++ b/src/solver.rs @@ -424,6 +424,19 @@ impl PositionSolver { let (lat_rad, lon_rad, height_m) = ecef2geodetic(sol.pos[0], sol.pos[1], sol.pos[2], Ellipsoid::WGS84); let (lat, lon) = (lat_rad.to_degrees(), lon_rad.to_degrees()); + // SVs RAIM dropped don't count toward the published fix size, and the + // names go in the log so a recurring bad SV is visible. + let used = meas.len() - sol.excluded.len(); + let excl = if sol.excluded.is_empty() { + String::new() + } else { + let names: Vec = sol + .excluded + .iter() + .map(|&i| meas[i].sv.to_string()) + .collect(); + format!(" raim-excl={}", names.join(",")) + }; { let mut st = self.pub_state.lock().unwrap(); st.latitude = lat; @@ -431,7 +444,7 @@ impl PositionSolver { st.height = height_m; st.hdop = sol.hdop; st.vdop = sol.vdop; - st.fix_sv_count = meas.len(); + st.fix_sv_count = used; } let mut sig: Vec = sol .sigma @@ -454,10 +467,10 @@ impl PositionSolver { log::warn!( "{}", format!( - "position fix: {lat:.6},{lon:.6} h={height_m:.1}m hdop={:.1} vdop={:.1} {}sv {sig}{isb} cdt={:.3}ms https://maps.google.com/?ll={lat},{lon}", + "position fix: {lat:.6},{lon:.6} h={height_m:.1}m hdop={:.1} vdop={:.1} {}sv {sig}{isb}{excl} cdt={:.3}ms https://maps.google.com/?ll={lat},{lon}", sol.hdop, sol.vdop, - meas.len(), + used, sol.cdt_m / SPEED_OF_LIGHT * 1e3 ) .green() diff --git a/src/wls.rs b/src/wls.rs index 6644167..4fa3fa6 100644 --- a/src/wls.rs +++ b/src/wls.rs @@ -1,13 +1,14 @@ -//! The live weighted-least-squares solver with an inter-system-bias state — -//! the workaround for gnss-rtk 0.8's equal weighting and single clock state. -//! See [`wls_solve`] for the revert condition once upstream ships weights. +//! The live weighted-least-squares solver with an inter-system-bias state and +//! RAIM fault detection & exclusion — the workaround for gnss-rtk 0.8's equal +//! weighting and single clock state. See [`wls_solve`] for the revert +//! condition once upstream ships weights. use gnss_rs::constellation::Constellation; use map_3d::{Ellipsoid, ecef2geodetic}; /// A live weighted-least-squares fix: position plus the estimated clock and -/// inter-system bias, the self-calibrated per-constellation sigmas, and -/// geometry DOPs from the unweighted normal matrix. +/// inter-system bias, the self-calibrated per-constellation sigmas, geometry +/// DOPs from the unweighted normal matrix, and any RAIM-excluded outliers. pub(crate) struct WlsSolution { pub pos: [f64; 3], pub cdt_m: f64, @@ -15,25 +16,60 @@ pub(crate) struct WlsSolution { pub sigma: std::collections::HashMap, pub hdop: f64, pub vdop: f64, + /// Indices (into the caller's measurement arrays) that RAIM dropped as + /// outliers before this solution was computed; empty for a clean pool. + pub excluded: Vec, } -/// Weighted Gauss-Newton solve over (x, y, z, c·dt, c·ISB_gal) — the live -/// solver. It adds the two things gnss-rtk 0.8 lacks for a mixed pool: -/// per-measurement weights (its measurement sigma is literally `1.0 // TODO`) -/// and an inter-system-bias state, so Galileo's common clock offset stops -/// leaking into position. Weights are self-calibrated: solve once -/// equal-weighted, set each constellation's sigma to its residual RMS, solve -/// again — no hand-tuned constants. **Workaround status**: once a gnss-rtk -/// release accepts measurement sigmas (and a second clock state), hand the -/// weighting back upstream and let this revert to a cross-check -/// (GNSS_SOLVER=rtk selects gnss-rtk meanwhile). +/// A-priori 1σ pseudorange measurement noise (m) for the RAIM integrity test. +/// Deliberately a *fixed nominal* value, not the self-calibrated weighting σ: +/// a gross outlier inflates the self-calibrated σ (a 220 km blunder drove +/// σ_GPS to ~68 km on the ION HackRF set), which would then mask the very fault +/// we are testing for. The test compares each residual against the noise a +/// healthy single-frequency fix *should* show. +const RAIM_SIGMA_M: f64 = 10.0; + +/// w-test gate: exclude the SV whose normalized residual exceeds this many +/// nominal σ. 6σ ⇒ a per-SV false-alarm rate ~2e-9, so a healthy SV is +/// essentially never dropped, while a km-scale blunder trips it by orders of +/// magnitude. (The denominator omits the residual-sensitivity factor √sᵢᵢ ≤ 1, +/// which only makes the test more conservative — never more trigger-happy.) +const RAIM_GATE: f64 = 6.0; + +/// State count for a pool: position+clock (4), plus the Galileo inter-system +/// bias (5) when both constellations are present with a redundant measurement. +/// The ISB state is only observable with both constellations and one spare. +fn state_count(active: &[usize], gal: &[bool]) -> usize { + let mixed = active.iter().any(|&i| gal[i]) && active.iter().any(|&i| !gal[i]); + if mixed && active.len() >= 5 { 5 } else { 4 } +} + +/// Weighted Gauss-Newton solve over (x, y, z, c·dt, c·ISB_gal) with RAIM +/// fault detection & exclusion — the live solver. It adds the things gnss-rtk +/// 0.8 lacks for a mixed pool: per-measurement weights (its measurement sigma +/// is literally `1.0 // TODO`), an inter-system-bias state so Galileo's common +/// clock offset stops leaking into position, and outlier rejection. Weights are +/// self-calibrated: solve once equal-weighted, set each constellation's sigma to +/// its residual RMS, solve again — no hand-tuned constants. **Workaround +/// status**: once a gnss-rtk release accepts measurement sigmas (and a second +/// clock state), hand the weighting back upstream and let this revert to a +/// cross-check (GNSS_SOLVER=rtk selects gnss-rtk meanwhile). +/// +/// RAIM: solve over the full pool, then — while the pool still has redundancy +/// (more measurements than states) — drop the satellite whose residual is the +/// largest multiple of [`RAIM_SIGMA_M`] when that exceeds [`RAIM_GATE`], and +/// re-solve. A single gross outlier (cross-correlation false lock, bad +/// ephemeris) is otherwise least-squares-smeared across the fix: on the ION +/// HackRF set one such SV (a ~220 km pseudorange blunder) pushed the fix 63 km +/// horizontally and 37 km up. Excluded indices land in [`WlsSolution::excluded`]. /// /// `meas[i]` is the fully corrected pseudorange + c·sv_clock (m), `svp[i]` /// the SV ECEF position at transmit time rotated into the reception frame -/// (Sagnac), `gal[i]` its constellation flag. `x0` seeds the iteration (last -/// fix, or zeros — Gauss-Newton converges from the geocentre for GNSS -/// geometry). Returns `None` for underdetermined pools or a non-finite -/// solution (caller falls back to gnss-rtk). +/// (Sagnac), `gal[i]` its constellation flag, `var0[i]` its broadcast +/// residual-error variance prior (m²). `x0` seeds the iteration (last fix, or +/// zeros — Gauss-Newton converges from the geocentre for GNSS geometry). +/// Returns `None` for underdetermined pools or a non-finite solution (caller +/// falls back to gnss-rtk). pub(crate) fn wls_solve( meas: &[f64], svp: &[[f64; 3]], @@ -41,13 +77,53 @@ pub(crate) fn wls_solve( var0: &[f64], x0: [f64; 3], ) -> Option { - use std::collections::HashMap; let n = meas.len(); - let mixed = gal.iter().any(|&g| g) && gal.iter().any(|&g| !g); - // The ISB state is only observable with both constellations present (and - // one redundant measurement); single-constellation pools solve 4 states. - let ns = if mixed && n >= 5 { 5 } else { 4 }; - if n < ns { + let mut active: Vec = (0..n).collect(); + let mut excluded: Vec = Vec::new(); + loop { + let (mut sol, resid) = solve_once(&active, meas, svp, gal, var0, x0)?; + let ns = state_count(&active, gal); + // With no redundancy there is nothing to test the residuals against; + // accept the solution as-is. + if active.len() <= ns { + sol.excluded = excluded; + return Some(sol); + } + // Worst normalized residual (a-priori σ plus this SV's broadcast prior). + let mut worst = (0usize, 0.0f64); + for (k, &r) in resid.iter().enumerate() { + let s2 = RAIM_SIGMA_M.powi(2) + var0[active[k]]; + let w = r.abs() / s2.sqrt(); + if w > worst.1 { + worst = (k, w); + } + } + if worst.1 > RAIM_GATE { + excluded.push(active[worst.0]); + active.remove(worst.0); + continue; + } + sol.excluded = excluded; + return Some(sol); + } +} + +/// One weighted Gauss-Newton solve over the `active` subset of the pool. Returns +/// the fix plus the per-SV post-fit residuals (m, in `active` order) the RAIM +/// loop tests. Indices in `active` address the full `meas`/`svp`/`gal`/`var0` +/// arrays. Weights are self-calibrated; see [`wls_solve`]. +fn solve_once( + active: &[usize], + meas: &[f64], + svp: &[[f64; 3]], + gal: &[bool], + var0: &[f64], + x0: [f64; 3], +) -> Option<(WlsSolution, Vec)> { + use std::collections::HashMap; + let na = active.len(); + let ns = state_count(active, gal); + if na < ns { return None; } @@ -89,7 +165,7 @@ pub(crate) fn wls_solve( // Normal equations A·dx = b for H_i = [−û, 1, gal_i] and // r_i = meas_i − (ρ_i + c·dt + gal_i·c·ISB), weight 1/σ². let (mut a, mut b) = ([[0.0f64; 5]; 5], [0.0f64; 5]); - for i in 0..n { + for &i in active { let d = [svp[i][0] - x[0], svp[i][1] - x[1], svp[i][2] - x[2]]; let rho = (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt(); let h = [ @@ -130,7 +206,7 @@ pub(crate) fn wls_solve( } // Self-calibrate: each constellation's residual RMS becomes its σ. let mut acc = HashMap::::new(); - for i in 0..n { + for &i in active { let d = [svp[i][0] - x[0], svp[i][1] - x[1], svp[i][2] - x[2]]; let rho = (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt(); let r = meas[i] - (rho + cdt + if gal[i] { isb } else { 0.0 }); @@ -146,10 +222,18 @@ pub(crate) fn wls_solve( return None; } + // Post-fit residuals (in `active` order) for the RAIM integrity test. + let mut resid = Vec::with_capacity(na); + for &i in active { + let d = [svp[i][0] - x[0], svp[i][1] - x[1], svp[i][2] - x[2]]; + let rho = (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt(); + resid.push(meas[i] - (rho + cdt + if gal[i] { isb } else { 0.0 })); + } + // Geometry DOPs from the *unweighted* normal matrix at the solution (DOP // is a geometry factor; weighting it would conflate signal quality in). let mut a = [[0.0f64; 5]; 5]; - for i in 0..n { + for &i in active { let d = [svp[i][0] - x[0], svp[i][1] - x[1], svp[i][2] - x[2]]; let rho = (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt(); let h = [ @@ -196,12 +280,16 @@ pub(crate) fn wls_solve( } } } - Some(WlsSolution { - pos: x, - cdt_m: cdt, - isb_m: isb, - sigma, - hdop: (qenu[0] + qenu[1]).max(0.0).sqrt(), - vdop: qenu[2].max(0.0).sqrt(), - }) + Some(( + WlsSolution { + pos: x, + cdt_m: cdt, + isb_m: isb, + sigma, + hdop: (qenu[0] + qenu[1]).max(0.0).sqrt(), + vdop: qenu[2].max(0.0).sqrt(), + excluded: Vec::new(), + }, + resid, + )) }