feat(gsdc2023): divergence-gated acceptance for the 2-stage residual re-solve

Adds fgo_two_stage_divergence_p95_m (BridgeConfig, default 0 = disabled) and
CLI --fgo-two-stage-divergence-p95-m. When > 0, the 2-stage re-solve only fires
on chunks whose pass-1 fixed-linearization residual p95 (over active rows)
exceeds the threshold — i.e. only chunks where pass-1 diverged. Healthy chunks
are left at the pass-1 result, so the re-solve introduces no perturbation there.

Motivation: a per-chunk diagnostic probe across win / wash / regression trips
showed the unconditional re-solve's only net win (lax-o) is split into two parts:
a divergence rescue on a couple of chunks where pass-1 blew up to multi-km
residuals (pass-1 p95 ~39 km), and pervasive marginal NLOS masking on healthy
chunks (p95 ~30 m). Every regression trip (lax-t/mtv-b/mtv-pe1 +10..11 cm) does
only the marginal masking on a healthy fit (p95 <= ~50 m). The two are
indistinguishable by the marginal residuals themselves, but the divergence
chunks stand alone by orders of magnitude (pass-1 p95 38840 m vs <= 53 m
everywhere else, position move 16908 m vs <= 3 m).

Gating on pass-1 divergence (p95 > 500 m) cleanly captures the rescue and skips
the marginal masking. An 8-trip A/B (div500 vs baseline) confirms: every +10 cm
regression goes bit-identical to baseline, and lax-o keeps -9.3 cm (the
divergence-rescue portion of its -54.6 cm unconditional win). The marginal-mask
bulk of lax-o's win is inseparable from the regressions, so the gated lever is a
clean zero-regression rescue (1 win / 0 reg / 7 wash) rather than the larger but
net-wash unconditional lever.

The Huber guard cannot do this (the rescue raises the robust cost: lax-o
cost_ratio 1.32), so divergence gating is the correct discriminator. Default off
keeps legacy behaviour; independent of the guard flag. 2 new unit tests.

Author: rsasaki0109

experiments/build_gsdc2023_bridge_submission.py

@@ -358,6 +358,9 @@ def build_config(args: argparse.Namespace) -> BridgeConfig:
         fgo_two_stage_residual_guard=bool(
             getattr(args, "fgo_two_stage_residual_guard", False)
         ),
+        fgo_two_stage_divergence_p95_m=float(
+            getattr(args, "fgo_two_stage_divergence_p95_m", 0.0)
+        ),
     )
     if bool(getattr(args, "taroz_marupaku", False)):
         cfg = apply_taroz_marupaku_preset(cfg)
@@ -619,6 +622,12 @@ def main(argv: list[str] | None = None) -> int:
         default=False,
         help="Gate the 2-stage re-solve behind a Huber trust-region cost (keep pass-2 only if the full-set Huber residual cost improves). OFF by default: the dense-urban win comes from dropping biased NLOS rows, which lowers position error but *raises* the Huber cost, so the guard rejects exactly the re-solves we want. Retained for experimentation.",
     )
+    parser.add_argument(
+        "--fgo-two-stage-divergence-p95-m",
+        type=float,
+        default=0.0,
+        help="Recommended acceptance gate for the 2-stage re-solve (0 = disabled). When > 0, the re-solve only fires on chunks whose pass-1 fixed-linearization residual p95 (over active rows) exceeds this many metres — i.e. only chunks where pass-1 diverged. Keeps the lax-o divergence-rescue win while skipping the marginal masking that causes +10 cm regressions on healthy chunks.",
+    )
     parser.add_argument("--stop-attitude-sigma-rad", type=float, default=0.0)
     parser.add_argument(
         "--taroz-fgo",

experiments/gsdc2023_bridge_config.py

@@ -295,6 +295,15 @@ class BridgeConfig:
     fgo_two_stage_residual_threshold_l5_m: float = 15.0
     fgo_two_stage_residual_min_keep: int = 5
     fgo_two_stage_residual_guard: bool = False
+    # Recommended acceptance gate for the 2-stage re-solve (0 = disabled).  When
+    # > 0, the re-solve is attempted only on chunks whose pass-1 fixed-
+    # linearization residual p95 (over active rows) exceeds this many metres —
+    # i.e. only chunks where pass-1 diverged.  A diagnostic probe showed the only
+    # net win (lax-o) comes from such diverged chunks (p95 ~39 km) while every
+    # regression trip masks marginal outliers on a healthy pass-1 fit (p95
+    # <= ~50 m), so gating on divergence keeps the rescue and drops the +10 cm
+    # regressions.
+    fgo_two_stage_divergence_p95_m: float = 0.0
     tdcp_weight_scale: float = DEFAULT_TDCP_WEIGHT_SCALE
     tdcp_l5_weight_scale: float = 1.0
     tdcp_geometry_correction: bool = DEFAULT_TDCP_GEOMETRY_CORRECTION
@@ -412,6 +421,9 @@ def __post_init__(self) -> None:
                 raise ValueError(f"{name} must be finite and > 0")
         if int(self.fgo_two_stage_residual_min_keep) < 4:
             raise ValueError("fgo_two_stage_residual_min_keep must be >= 4")
+        divergence_p95 = float(self.fgo_two_stage_divergence_p95_m)
+        if not np.isfinite(divergence_p95) or divergence_p95 < 0.0:
+            raise ValueError("fgo_two_stage_divergence_p95_m must be finite and >= 0")
         for name in (
             "stop_velocity_huber_k",
             "stop_position_huber_k",

experiments/gsdc2023_raw_bridge.py

@@ -2324,25 +2324,37 @@ def two_stage_residual_resolve_vd(
     threshold_l5_m: float,
     min_keep: int,
     guard: bool = True,
+    divergence_p95_m: float = 0.0,
     vd_kwargs: dict,
 ) -> tuple[int, float]:
     """taroz-style 2-stage residual re-solve for the VD path. Runs ``solver_fn``
     (the VD solver), recomputes the fixed-linearization residual at the converged
     ``state``, masks outliers above threshold, and re-solves warm-started.
 
+    ``divergence_p95_m`` is the recommended acceptance criterion (default 0 =
+    disabled). When > 0, the re-solve is attempted ONLY on chunks whose pass-1
+    fit has diverged — i.e. the 95th percentile of the pass-1 fixed-linearization
+    residual over active rows exceeds the threshold. A diagnostic probe across
+    win / wash / regression trips showed the only net win (lax-o) comes from a
+    handful of chunks where pass-1 diverged to multi-km residuals (p95 ~39 km),
+    while every regression trip masks only marginal outliers on a healthy pass-1
+    fit (p95 <= ~50 m); gating on pass-1 divergence keeps the rescue and skips
+    the perturbations that cause the +10 cm regressions. The chunk is left
+    untouched (pass-1 kept) when its pass-1 fit is healthy.
+
     When ``guard`` is True the re-solve is kept only if it lowers the full-set
     Huber guard cost (a trust region), otherwise ``state`` is restored to the
     pass-1 result. **The guard is OFF by default in production**: the dense-urban
     win comes from dropping systematically biased NLOS measurements, which snaps
     the position toward truth but *raises* the robust residual cost (the dropped
     outliers are re-added at their original weight, Huber-capped), so the guard
     rejects exactly the re-solves we want. See PR / memory notes. The guard is
-    retained for experimentation only.
+    retained for experimentation only and is independent of the divergence gate.
 
     Returns ``(iters, mse)`` and mutates ``state`` in place (matching the native
     solver contract). A no-op returning the pass-1 result when fixed
-    linearization inputs are absent, nothing is masked, the re-solve fails, or the
-    guard (when enabled) rejects it.
+    linearization inputs are absent, the pass-1 fit is healthy (divergence gate),
+    nothing is masked, the re-solve fails, or the guard (when enabled) rejects it.
     """
     iters1, mse1 = solver_fn(sat_ecef, pseudorange, weights, state, **vd_kwargs)
     ref = vd_kwargs.get("pr_linearization_ref_ecef")
@@ -2356,6 +2368,14 @@ def two_stage_residual_resolve_vd(
         sys_kind, pseudorange.shape, threshold_l1_m, threshold_l5_m
     )
     resid1 = _pr_linearized_residual(pseudorange, los, ref, state, sys_kind, n_clock)
+    if float(divergence_p95_m) > 0.0:
+        w = np.asarray(weights, dtype=np.float64)
+        active = np.isfinite(w) & (w > 0.0)
+        ar = np.abs(resid1)[active]
+        ar = ar[np.isfinite(ar)]
+        pass1_p95 = float(np.percentile(ar, 95)) if ar.size else 0.0
+        if pass1_p95 <= float(divergence_p95_m):
+            return iters1, mse1  # pass-1 fit is healthy: skip the re-solve
     new_w, n_masked = _pr_two_stage_mask(resid1, weights, thresholds, int(min_keep))
     if n_masked == 0:
         return iters1, mse1
@@ -2687,6 +2707,7 @@ def run_fgo_chunked(
     fgo_two_stage_residual_threshold_l5_m: float = 15.0,
     fgo_two_stage_residual_min_keep: int = 5,
     fgo_two_stage_residual_guard: bool = False,
+    fgo_two_stage_divergence_p95_m: float = 0.0,
     fgo_seed_state: np.ndarray | None = None,
 ) -> "ChunkedFgoRun":
     n_epoch = batch.sat_ecef.shape[0]
@@ -3251,6 +3272,7 @@ def run_fgo_chunked(
                             threshold_l5_m=fgo_two_stage_residual_threshold_l5_m,
                             min_keep=fgo_two_stage_residual_min_keep,
                             guard=fgo_two_stage_residual_guard,
+                            divergence_p95_m=fgo_two_stage_divergence_p95_m,
                             vd_kwargs=vd_kwargs,
                         )
                     else:
@@ -3708,6 +3730,9 @@ def solve_trip(
     fgo_run_kwargs["fgo_two_stage_residual_guard"] = bool(
         getattr(config, "fgo_two_stage_residual_guard", False)
     )
+    fgo_run_kwargs["fgo_two_stage_divergence_p95_m"] = float(
+        getattr(config, "fgo_two_stage_divergence_p95_m", 0.0)
+    )
     taroz_candidate_sources = _taroz_fgo_candidate_sources_enabled(config)
     taroz_candidate_base_run_kwargs = dict(fgo_run_kwargs)
     effective_trip_type: str | None = None

tests/test_two_stage_residual_resolve.py

@@ -190,6 +190,45 @@ def test_resolve_keeps_pass2_when_guard_disabled():
     assert state[0, 6] == -50.0  # pass-2 state kept despite worse Huber cost
 
 
+def test_divergence_gate_skips_healthy_pass1():
+    # One row just over the 20 m mask threshold (would normally mask + re-solve),
+    # but the pass-1 residual p95 is low (~18 m) -> the divergence gate skips the
+    # re-solve entirely, so pass-1 is kept untouched.
+    z = np.asarray([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 21.0]], dtype=np.float64)
+    w = np.ones((1, 7), dtype=np.float64)
+    state = _state([0.0, 0.0, 0.0], c0=0.0)
+    los = np.zeros((1, 7, 3)); ref = np.zeros((1, 3))
+    kw = dict(pr_linearization_ref_ecef=ref, pr_linearization_los_ecef=los,
+              sys_kind=None, n_clock=1)
+    solver = _MockSolver(pass1_c0=0.0, pass2_c0=1.0)
+    iters, _mse = two_stage_residual_resolve_vd(
+        solver, None, z, w, state,
+        threshold_l1_m=20.0, threshold_l5_m=15.0, min_keep=5,
+        divergence_p95_m=500.0, vd_kwargs=kw)
+    assert solver.calls == 1  # healthy pass-1 -> gate skips the re-solve
+    assert iters == 5
+    assert state[0, 6] == 0.0
+
+
+def test_divergence_gate_fires_on_diverged_pass1():
+    # Pass-1 diverged: every residual ~600 m (p95 > 500 m gate) -> the re-solve
+    # fires even with the divergence gate enabled.
+    z = np.full((1, 7), 600.0, dtype=np.float64)
+    w = np.ones((1, 7), dtype=np.float64)
+    state = _state([0.0, 0.0, 0.0], c0=0.0)
+    los = np.zeros((1, 7, 3)); ref = np.zeros((1, 3))
+    kw = dict(pr_linearization_ref_ecef=ref, pr_linearization_los_ecef=los,
+              sys_kind=None, n_clock=1)
+    solver = _MockSolver(pass1_c0=0.0, pass2_c0=1.0)
+    iters, _mse = two_stage_residual_resolve_vd(
+        solver, None, z, w, state,
+        threshold_l1_m=20.0, threshold_l5_m=15.0, min_keep=5,
+        divergence_p95_m=500.0, vd_kwargs=kw)
+    assert solver.calls == 2  # diverged pass-1 -> re-solve fires
+    assert iters == 12
+    assert state[0, 6] == 1.0
+
+
 def test_resolve_noop_without_fixed_linearization():
     z = np.asarray([[100.0, 100.0, 100.0, 100.0, 100.0, 100.0]], dtype=np.float64)
     w = np.ones((1, 6), dtype=np.float64)