wls: add RAIM fault detection & exclusion to reject outlier SVs

src/solver.rs

@@ -424,14 +424,27 @@ 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<String> = 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;
             st.longitude = lon;
             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<String> = 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()

src/wls.rs

@@ -1,53 +1,129 @@
-//! 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,
     pub isb_m: f64,
     pub sigma: std::collections::HashMap<Constellation, f64>,
     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<usize>,
 }
 
-/// 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]],
     gal: &[bool],
     var0: &[f64],
     x0: [f64; 3],
 ) -> Option<WlsSolution> {
-    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<usize> = (0..n).collect();
+    let mut excluded: Vec<usize> = 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<f64>)> {
+    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::<Constellation, (f64, usize)>::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,
+    ))
 }