Add reference pose-arbitration consumer for the calibrated covariance (#72) (#96)

The competitive-roadmap 'Next' gate asks that the covariance be 'documented
enough to be consumed by fusion or arbitration logic'. docs/pose_covariance.md
already says the calibrated error_floor covariance is usable for arbitration
*provided the consumer also watches /alignment_status*, but shipped no reference
implementation of such a consumer. This adds one, in the Phase 3 fixture +
ROS-free policy + regression-test style.

Two consumers make the contract explicit:

* covariance_only -- trusts purely on the published 2-sigma radius. On the
  error_floor model this is *actively* unsafe: a degenerate-geometry pose has low
  fitness, and low fitness yields a TIGHT covariance, so this consumer assigns
  full trust to exactly the bias-tail poses that carry multi-metre error. On the
  calibration fixtures it makes 4 dangerous trusts.
* covariance_plus_category -- the reference consumer. It vetoes first on the
  diagnostics covariance cannot represent (an active degeneracy/overload
  failure_category, a reject streak, or an open reinitialization request) and
  only then weights the surviving geometrically-sound pose by its 2-sigma radius.
  0 dangerous trusts.

Fixtures are grounded in the calibration data (calibration_2026_06_12.json) and
the documented bias tail (4.2 m at fitness 0.5-1.0 on Koide). Each fixture
declares the expected verdict from both consumers so policy and fixtures cannot
drift. scripts/demo_pose_arbitration.py prints the side-by-side scorecard and
exits non-zero if the reference consumer ever makes a dangerous trust.
test/test_pose_arbitration_policy.py: 5 tests green. docs/pose_covariance.md gets
a 'Reference arbitration consumer' section.

Author: rsasaki0109

docs/pose_covariance.md

@@ -128,6 +128,47 @@ Cannot:
 | Arbitration between two pose sources | usable for 2σ-level comparisons; never trust covariance alone in degenerate geometry |
 | Safety-critical gating | covariance is not sufficient; use `/alignment_status` reject reasons and `reinitialization_requested` |
 
+## Reference arbitration consumer
+
+The guidance above ("usable, but also watch `/alignment_status`") is backed by a
+runnable, tested reference consumer rather than left as prose:
+
+- policy: `experiments/pose_arbitration/policy.py`
+- fixtures: `experiments/pose_arbitration/fixtures/`
+- regression test: `test/test_pose_arbitration_policy.py`
+- demo: `python3 scripts/demo_pose_arbitration.py`
+
+It contrasts two consumers of a published pose:
+
+| Consumer | Inputs | Result on the calibration fixtures |
+| --- | --- | --- |
+| `covariance_only` | published 2σ radius only | **4 dangerous trusts** — fooled by the bias tail |
+| `covariance_plus_category` | 2σ radius **+** `failure_category`, `consecutive_rejected_updates`, `reinitialization_requested` | **0 dangerous trusts** |
+
+The decisive case is the **bias tail**: a degenerate-geometry pose has *low*
+fitness, and in `error_floor` mode low fitness yields a *tight* covariance, so a
+covariance-only consumer assigns full trust to exactly the poses carrying
+multi-metre error. The reference consumer first vetoes on the diagnostics
+covariance cannot represent — an active degeneracy/overload category
+(`weak_overlap` is the tell), a reject streak, or an open reinitialization
+request — and only then uses the 2σ radius to weight the surviving,
+geometrically-sound pose. Demo output:
+
+```text
+fixture                                          true_err  cov2sig     covariance_only  covariance_plus_category
+degenerate_low_fitness_bias_tail_should_reject      4.20m    0.56m       trust w=1.00!         reject w=0.00
+healthy_tight_should_trust                          0.18m    0.46m       trust w=1.00          trust  w=1.00
+loose_covariance_should_downweight                  0.24m    1.20m  downweight w=0.83     downweight w=0.83
+reinit_requested_should_reject                     12.00m    0.60m       trust w=1.00!         reject w=0.00
+reject_streak_should_reject                         3.10m    0.50m       trust w=1.00!         reject w=0.00
+stale_prediction_jump_should_reject                 7.50m    0.60m       trust w=1.00!         reject w=0.00
+```
+
+This is the consumable-covariance contract: covariance decides *how much* to
+trust a geometrically-sound pose; `/alignment_status` decides *whether the pose
+is sound at all*. Either signal alone is insufficient; together they are safe for
+fusion and arbitration.
+
 ## Related docs
 
 - [troubleshooting.md](troubleshooting.md) — `/alignment_status` field guide,

experiments/pose_arbitration/__init__.py

@@ -0,0 +1,18 @@
+{
+  "name": "degenerate_low_fitness_bias_tail_should_reject",
+  "description": "THE money case. Degenerate geometry (e.g. a long corridor / open field) produces an accepted pose with LOW fitness (~0.8) yet multi-metre true error (4.2 m, the value docs/pose_covariance.md records for fitness 0.5-1.0 on Koide). In error_floor mode low fitness yields a TIGHT covariance (std ~0.28 m, 2sigma 0.56 m), so the covariance-only consumer assigns it FULL trust -- it is fooled into maximal confidence on the most-wrong pose. The degeneracy shows up as weak_overlap in /alignment_status, which the reference consumer vetoes on. This fixture is exactly the limit the covariance doc says covariance cannot bound and a consumer must watch failure_category for.",
+  "source": "docs/pose_covariance.md 'systematically biased tail' (4.2 m at fitness 0.5-1.0, Koide); weak_overlap from alignment_failure_taxonomy.hpp",
+  "expectation": {
+    "covariance_only": "trust",
+    "covariance_plus_category": "reject"
+  },
+  "sample": {
+    "index": 77,
+    "cov_xy_std_m": 0.28,
+    "cov_yaw_std_deg": 2.8,
+    "failure_category": "weak_overlap",
+    "consecutive_rejected_updates": 0,
+    "reinitialization_requested": false,
+    "abs_xy_error_m": 4.2
+  }
+}

experiments/pose_arbitration/fixtures/degenerate_low_fitness_bias_tail_should_reject.json

@@ -0,0 +1,18 @@
+{
+  "name": "healthy_tight_should_trust",
+  "description": "Real healthy tracking: low fitness -> tight error_floor covariance (std ~0.23 m, 2sigma 0.46 m) and a small true error. Both consumers should TRUST -- the calibrated covariance is doing its job here.",
+  "source": "koide outdoor_hard_01a calibration (calibration_2026_06_12.json, koide_baseline fitness<0.5 bin: p68 0.18 m)",
+  "expectation": {
+    "covariance_only": "trust",
+    "covariance_plus_category": "trust"
+  },
+  "sample": {
+    "index": 10,
+    "cov_xy_std_m": 0.23,
+    "cov_yaw_std_deg": 2.3,
+    "failure_category": "healthy",
+    "consecutive_rejected_updates": 0,
+    "reinitialization_requested": false,
+    "abs_xy_error_m": 0.18
+  }
+}

experiments/pose_arbitration/fixtures/healthy_tight_should_trust.json

@@ -0,0 +1,18 @@
+{
+  "name": "loose_covariance_should_downweight",
+  "description": "Accepted but middling match: fitness ~4 -> error_floor loosens to std ~0.6 m (2sigma 1.2 m), above the 1.0 m trust radius. Both consumers should DOWNWEIGHT, not trust. This is the regime where covariance alone IS informative -- a worse match correctly produces a looser, lower-weight pose.",
+  "source": "koide outdoor_hard_01a calibration (fitness 2-4 bin: p68 0.17-0.24 m); error_floor std = 0.2 + 0.1*fitness",
+  "expectation": {
+    "covariance_only": "downweight",
+    "covariance_plus_category": "downweight"
+  },
+  "sample": {
+    "index": 42,
+    "cov_xy_std_m": 0.6,
+    "cov_yaw_std_deg": 6.0,
+    "failure_category": "healthy",
+    "consecutive_rejected_updates": 0,
+    "reinitialization_requested": false,
+    "abs_xy_error_m": 0.24
+  }
+}

experiments/pose_arbitration/fixtures/loose_covariance_should_downweight.json

@@ -0,0 +1,18 @@
+{
+  "name": "reinit_requested_should_reject",
+  "description": "The localizer has raised /reinitialization_requested -- it has judged tracking lost. No covariance value on a pose published in this state is trustworthy for fusion. covariance-only ignores the signal; the reference consumer rejects unconditionally.",
+  "source": "alignment_status reinitialization_requested (the G3 recovery trigger)",
+  "expectation": {
+    "covariance_only": "trust",
+    "covariance_plus_category": "reject"
+  },
+  "sample": {
+    "index": 240,
+    "cov_xy_std_m": 0.3,
+    "cov_yaw_std_deg": 3.0,
+    "failure_category": "bad_match",
+    "consecutive_rejected_updates": 2,
+    "reinitialization_requested": true,
+    "abs_xy_error_m": 12.0
+  }
+}

experiments/pose_arbitration/fixtures/reinit_requested_should_reject.json

@@ -0,0 +1,18 @@
+{
+  "name": "reject_streak_should_reject",
+  "description": "The last published pose still looks clean (healthy category, tight covariance), but the localizer has rejected several scans in a row since -- tracking is degrading and the held pose is going stale. covariance-only keeps trusting the last tight covariance; the reference consumer stops trusting once the reject streak crosses max_consecutive_rejects.",
+  "source": "alignment_status consecutive_rejected_updates; Phase 3 lesson that drift hides behind a clean last-accepted pose",
+  "expectation": {
+    "covariance_only": "trust",
+    "covariance_plus_category": "reject"
+  },
+  "sample": {
+    "index": 200,
+    "cov_xy_std_m": 0.25,
+    "cov_yaw_std_deg": 2.5,
+    "failure_category": "healthy",
+    "consecutive_rejected_updates": 5,
+    "reinitialization_requested": false,
+    "abs_xy_error_m": 3.1
+  }
+}

experiments/pose_arbitration/fixtures/reject_streak_should_reject.json

@@ -0,0 +1,18 @@
+{
+  "name": "stale_prediction_jump_should_reject",
+  "description": "A long gap since the last accepted pose makes the prediction seed stale; the pose locks onto a wrong basin with low fitness (tight covariance ~0.3 m) but a large true error. stale_prediction is active. covariance-only trusts the tight covariance; the reference consumer vetoes on the category.",
+  "source": "alignment_failure_taxonomy.hpp stale_prediction (gap > stale_prediction_min_gap_sec)",
+  "expectation": {
+    "covariance_only": "trust",
+    "covariance_plus_category": "reject"
+  },
+  "sample": {
+    "index": 120,
+    "cov_xy_std_m": 0.3,
+    "cov_yaw_std_deg": 3.0,
+    "failure_category": "stale_prediction",
+    "consecutive_rejected_updates": 0,
+    "reinitialization_requested": false,
+    "abs_xy_error_m": 7.5
+  }
+}

experiments/pose_arbitration/fixtures/stale_prediction_jump_should_reject.json

@@ -0,0 +1,78 @@
+"""Interface for the reference pose-arbitration consumer (issue #72 follow-up).
+
+`docs/pose_covariance.md` says the calibrated `error_floor` covariance is
+"usable for 2-sigma-level comparisons" by fusion / arbitration logic, but adds a
+hard caveat: *never trust covariance alone in degenerate geometry* -- a pose can
+be accepted with multi-metre error at *low* fitness, and in `error_floor` mode a
+low fitness yields a *small* covariance, so a covariance-only consumer is
+actively misled into high confidence exactly when it is most wrong. The doc tells
+consumers to also watch `/alignment_status` (`failure_category`,
+`consecutive_rejected_updates`, `reinitialization_requested`) -- but ships no
+reference implementation of a consumer that does so.
+
+This module is that reference consumer, built ROS-free in the Phase 3 fixture +
+policy + regression-test style so the claim "covariance is consumable by
+arbitration" is backed by a runnable, tested artifact instead of prose.
+
+Each consumer maps a published pose plus its alignment diagnostics to a trust
+verdict (TRUST / DOWNWEIGHT / REJECT) and a fusion weight. The fixtures carry the
+ground-truth `abs_xy_error_m` so the demo can score whether a verdict was
+*correct* (trusting a pose whose true error exceeds the trust radius is the
+failure we are guarding against).
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Protocol
+
+# Failure categories, mirroring include/lidar_localization/alignment_failure_taxonomy.hpp
+# so fixtures can be copied from recorded /alignment_status verbatim.
+CATEGORY_HEALTHY = "healthy"
+CATEGORY_MISSING_MAP = "missing_map"
+CATEGORY_MISSING_INITIAL_POSE = "missing_initial_pose"
+CATEGORY_WEAK_OVERLAP = "weak_overlap"
+CATEGORY_BAD_MATCH = "bad_match"
+CATEGORY_STALE_PREDICTION = "stale_prediction"
+CATEGORY_OVERLOAD = "overload"
+
+# Trust verdicts.
+TRUST = "trust"
+DOWNWEIGHT = "downweight"
+REJECT = "reject"
+
+
+@dataclass(frozen=True)
+class PoseSample:
+    """One published `/pcl_pose` as a downstream fusion consumer sees it.
+
+    The covariance fields are the *published* standard deviations (the square
+    roots of the diagonal terms), i.e. what `error_floor` mode emitted -- a
+    consumer never sees the true error, only this covariance and the diagnostics.
+    ``abs_xy_error_m`` is ground-truth-only and is used solely to *score* a
+    verdict in tests / the demo; a real consumer must not have it.
+    """
+
+    index: int
+    cov_xy_std_m: float
+    cov_yaw_std_deg: float
+    failure_category: str
+    consecutive_rejected_updates: int
+    reinitialization_requested: bool
+    # Ground-truth label for scoring only (not an input to any consumer).
+    abs_xy_error_m: float
+
+
+@dataclass(frozen=True)
+class ArbitrationVerdict:
+    trust: str          # TRUST / DOWNWEIGHT / REJECT
+    weight: float       # fusion weight in [0, 1]
+    reason: str
+
+
+class PoseConsumer(Protocol):
+    name: str
+    design: str
+
+    def decide(self, sample: PoseSample) -> ArbitrationVerdict:
+        ...

experiments/pose_arbitration/interface.py

@@ -0,0 +1,151 @@
+"""Reference pose-arbitration consumers for the calibrated covariance.
+
+Two consumers are defined so the comparison is explicit:
+
+* ``CovarianceOnlyConsumer`` -- the naive baseline the doc warns against: it
+  trusts a pose purely on its published 2-sigma radius. On the calibrated
+  ``error_floor`` model this is *actively* unsafe, because a degenerate-geometry
+  pose has low fitness, and low fitness yields a *small* covariance, so this
+  consumer assigns highest confidence to exactly the biased-tail poses that carry
+  multi-metre error. It exists to demonstrate the failure, not to be used.
+
+* ``CovariancePlusCategoryConsumer`` -- the reference consumer the doc's
+  guidance describes: it still *consumes* the covariance (the 2-sigma radius is
+  the trust gate and sets the fusion weight), but it first vetoes on the
+  diagnostics that covariance cannot represent -- an active degeneracy/overload
+  category (``weak_overlap`` is the tell for the bias tail), a reject streak, or
+  an open reinitialization request. Covariance decides *how much* to trust a
+  geometrically-sound pose; the category decides *whether the pose is
+  geometrically sound at all*.
+
+Both are pure and deterministic; ``test_pose_arbitration_policy.py`` pins that
+the covariance-only consumer trusts the bias tail (the failure) and the reference
+consumer rejects it (the fix), while both agree on the healthy and
+covariance-loosened cases.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+from experiments.pose_arbitration.interface import (
+    ArbitrationVerdict,
+    CATEGORY_HEALTHY,
+    DOWNWEIGHT,
+    PoseSample,
+    REJECT,
+    TRUST,
+)
+
+# Categories that mean "the geometry/timing behind this pose is not sound", so a
+# tight covariance on it is not trustworthy regardless of its value. bad_match is
+# included for completeness though an accepted, published pose is rarely flagged
+# bad_match (its fitness cleared the gate); weak_overlap / stale_prediction /
+# overload routinely co-occur with an accepted-but-degenerate pose.
+_UNSOUND_CATEGORIES = frozenset({
+    "weak_overlap",
+    "bad_match",
+    "stale_prediction",
+    "overload",
+    "missing_map",
+    "missing_initial_pose",
+})
+
+
+@dataclass(frozen=True)
+class ArbitrationParams:
+    # A pose is trustworthy for tight fusion only if its 2-sigma xy radius is
+    # within this bound (metres). Above it the pose is down-weighted, not vetoed.
+    trust_xy_2sigma_m: float = 1.0
+    # Down-weighted (rather than trusted) once the 2-sigma radius exceeds the
+    # trust bound; rejected outright past this far looser bound.
+    reject_xy_2sigma_m: float = 6.0
+    # A reject streak at or above this length means tracking is degrading even if
+    # the last accepted pose looks clean; stop trusting it for fusion.
+    max_consecutive_rejects: int = 3
+
+
+def _two_sigma_xy(sample: PoseSample) -> float:
+    return 2.0 * sample.cov_xy_std_m
+
+
+class CovarianceOnlyConsumer:
+    """Trust purely on the published 2-sigma radius (the unsafe baseline)."""
+
+    name = "covariance_only"
+    design = (
+        "Trust if 2-sigma xy radius <= trust bound; down-weight up to the reject "
+        "bound; reject beyond it. Ignores failure_category -- so on the "
+        "error_floor model it is fooled by tight covariance on low-fitness "
+        "degenerate poses."
+    )
+
+    def __init__(self, params: ArbitrationParams | None = None) -> None:
+        self.params = params or ArbitrationParams()
+
+    def decide(self, sample: PoseSample) -> ArbitrationVerdict:
+        two_sigma = _two_sigma_xy(sample)
+        if two_sigma <= self.params.trust_xy_2sigma_m:
+            return ArbitrationVerdict(
+                TRUST, 1.0, "2sigma_within_trust_radius")
+        if two_sigma <= self.params.reject_xy_2sigma_m:
+            weight = self.params.trust_xy_2sigma_m / two_sigma
+            return ArbitrationVerdict(DOWNWEIGHT, weight, "2sigma_loose")
+        return ArbitrationVerdict(REJECT, 0.0, "2sigma_beyond_reject_radius")
+
+
+class CovariancePlusCategoryConsumer:
+    """Consume covariance, but veto on diagnostics it cannot represent."""
+
+    name = "covariance_plus_category"
+    design = (
+        "Veto first on failure_category (degeneracy/overload), reject streak, or "
+        "an open reinitialization request -- the bias-tail signals covariance "
+        "cannot encode -- then use the 2-sigma radius to weight the surviving, "
+        "geometrically-sound pose. This is the consumer docs/pose_covariance.md "
+        "prescribes."
+    )
+
+    def __init__(self, params: ArbitrationParams | None = None) -> None:
+        self.params = params or ArbitrationParams()
+
+    def decide(self, sample: PoseSample) -> ArbitrationVerdict:
+        # 1) Diagnostics veto -- these are the cases the calibrated covariance is
+        #    documented as unable to bound (the systematically biased tail).
+        if sample.reinitialization_requested:
+            return ArbitrationVerdict(REJECT, 0.0, "reinitialization_requested")
+        if sample.consecutive_rejected_updates >= self.params.max_consecutive_rejects:
+            return ArbitrationVerdict(REJECT, 0.0, "reject_streak")
+        if sample.failure_category in _UNSOUND_CATEGORIES:
+            return ArbitrationVerdict(
+                REJECT, 0.0, "unsound_category:%s" % sample.failure_category)
+        if sample.failure_category != CATEGORY_HEALTHY:
+            # Unknown / future category: be conservative, do not fully trust.
+            return ArbitrationVerdict(
+                DOWNWEIGHT, 0.5, "unknown_category:%s" % sample.failure_category)
+
+        # 2) Geometrically sound -- now the covariance is meaningful, weight by it.
+        two_sigma = _two_sigma_xy(sample)
+        if two_sigma <= self.params.trust_xy_2sigma_m:
+            return ArbitrationVerdict(TRUST, 1.0, "healthy_2sigma_within_trust")
+        if two_sigma <= self.params.reject_xy_2sigma_m:
+            weight = self.params.trust_xy_2sigma_m / two_sigma
+            return ArbitrationVerdict(DOWNWEIGHT, weight, "healthy_2sigma_loose")
+        return ArbitrationVerdict(REJECT, 0.0, "2sigma_beyond_reject_radius")
+
+
+def verdict_is_correct(sample: PoseSample, verdict: ArbitrationVerdict,
+                       trust_radius_m: float) -> bool:
+    """Score a verdict against ground truth.
+
+    A verdict is *wrong* only when it TRUSTs (weight 1.0) a pose whose true xy
+    error exceeds the trust radius -- that is the dangerous outcome we guard
+    against (feeding a confidently-wrong pose into fusion). DOWNWEIGHT and REJECT
+    are conservative and counted correct regardless of the true error; trusting a
+    genuinely accurate pose is likewise correct.
+    """
+    trusted = verdict.trust == TRUST
+    accurate = sample.abs_xy_error_m <= trust_radius_m
+    if trusted and not accurate:
+        return False
+    return True