Add per-LH innovation gate for reflection rejection (TE-PROC-039)

When light-outlier-threshold > 0, compute the pre-update RMS innovation
for each lighthouse batch via a dry-run of map_light_data (no state change).
If rms_innovation > threshold * light_residuals_all, skip that LH for the
current sync cycle.

Unlike the EWMA-history approach, this fires on the frame of the reflection
itself rather than requiring multiple bad frames to accumulate. Disabled by
default (threshold=0); a starting value of 5.0 is recommended.

Adds 6 OutlierGate property tests to residual_cascade_props.c covering:
disabled states (threshold=0, cold start), fire/no-fire on either side of
the threshold, RMS correctness, and single-spike detection.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Author: Mark Stalzer

docs/specs/tracking-engine-specs.md

@@ -39,6 +39,7 @@ Prefix: **TE**
 - [x] **TE-PROC-036**: While the tracked object is stationary (IMU variance below `kalman-stationary-*` thresholds), the system shall apply a Zero Velocity Update pseudo-measurement forcing velocity and acceleration toward zero.
 - [x] **TE-PROC-037**: When tracking is lost (no valid measurements for the configured timeout period), the system shall reset the Kalman state covariance to reflect high uncertainty.
 - [x] **TE-PROC-038**: The system shall scale each lighthouse's observation noise covariance R by the ratio of that lighthouse's EWMA residual to the fleet mean residual, so that lighthouses with above-average residuals receive proportionally less Kalman weight.
+- [x] **TE-PROC-039**: Where `light-outlier-threshold` is > 0, the system shall compute the pre-update RMS innovation for each lighthouse batch and skip that lighthouse's Kalman update for the current sync cycle if the RMS exceeds `light-outlier-threshold × light_residuals_all`.
 
 ## Kalman Lighthouse Tracker
 

src/survive_kalman_tracker.c

@@ -46,6 +46,9 @@ STRUCT_CONFIG_SECTION(SurviveKalmanTracker)
 					   "Minimum observations to allow light data into the kalman filter", 16, t->light_required_obs)
 
     STRUCT_CONFIG_ITEM("light-max-error",  "Maximum error to integrate into lightcap", -1, t->lightcap_max_error)
+    STRUCT_CONFIG_ITEM("light-outlier-threshold",
+                       "Per-LH RMS innovation gate: skip a LH batch if its pre-update RMS residual "
+                       "exceeds this multiple of light_residuals_all (0 = disabled)", 0, t->light_outlier_threshold)
     STRUCT_CONFIG_ITEM("kalman-light-variance",  "Variance of raw light sensor readings", 1e-2, t->light_var)
     STRUCT_CONFIG_ITEM("obs-cov-scale",  "Covariance matrix scaling for obs",
                        1, t->obs_cov_scale)
@@ -447,6 +450,28 @@ void survive_kalman_tracker_integrate_saved_light(SurviveKalmanTracker *tracker,
 				.tracker = tracker,
 			};
 
+			// @spec TE-PROC-039
+			// Per-LH innovation gate: compute pre-update RMS residual via a dry-run
+			// of the measurement model (no state change). If it exceeds
+			// light_outlier_threshold × light_residuals_all, the LH is producing
+			// measurements too far from the current state estimate to be trusted
+			// (typically a reflection or severe miscalibration) and is skipped for
+			// this sync cycle.
+			if (tracker->light_outlier_threshold > 0 && tracker->light_residuals_all > 0) {
+				CN_CREATE_STACK_VEC(y_dry, cnt);
+				bool dry_ok = map_light_data(&cbctx, &Z, &tracker->model.state, &y_dry, NULL);
+				if (dry_ok) {
+					const FLT *yv = cn_as_const_vector(&y_dry);
+					FLT sq = 0;
+					for (int i = 0; i < cnt; i++) sq += yv[i] * yv[i];
+					FLT rms = FLT_SQRT(sq / cnt);
+					if (rms > tracker->light_outlier_threshold * tracker->light_residuals_all) {
+						tracker->stats.lightcap_model_dropped++;
+						continue;
+					}
+				}
+			}
+
 			SurviveObject *so = tracker->so;
 
 			// @spec TE-PROC-038

src/survive_kalman_tracker.h

@@ -140,6 +140,7 @@ typedef struct SurviveKalmanTracker {
 	FLT Lightcap_R;
 
 	FLT lightcap_max_error;
+	FLT light_outlier_threshold;
 	int light_rampin_length;
 	bool use_error_for_lh_pos;
 

src/test_cases/residual_cascade_props.c

@@ -964,3 +964,182 @@ TEST(AdaptiveR, PerLhEWMAConvergesIndependently) {
 	}
 	return 0;
 }
+
+// ── 9. Per-LH Innovation Gate Properties (TE-PROC-039) ──────────
+//
+// The outlier gate in survive_kalman_tracker.c fires when:
+//   rms_innovation > light_outlier_threshold * light_residuals_all
+//
+// These tests exercise the gate decision logic in isolation. They do not
+// call map_light_data (which requires the full Kalman machinery) — instead
+// they test the RMS computation and threshold comparison that wraps the
+// dry-run call.
+
+// Reproduce the gate decision from survive_kalman_tracker.c:
+//   gate fires when rms > threshold * mean_res  (and threshold > 0, mean_res > 0)
+static bool outlier_gate_fires(double rms, double threshold, double mean_res) {
+	if (threshold <= 0 || mean_res <= 0)
+		return false;
+	return rms > threshold * mean_res;
+}
+
+// Reproduce the RMS computation:
+//   rms = sqrt(sum(y[i]^2) / cnt)
+static double compute_rms(const double *y, int cnt) {
+	double sq = 0;
+	for (int i = 0; i < cnt; i++) sq += y[i] * y[i];
+	return sqrt(sq / cnt);
+}
+
+// Property 1: Gate is disabled when threshold <= 0 (default off)
+TEST(OutlierGate, DisabledWhenThresholdZero) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double rms       = rand_range(0.0, 100.0);
+		double threshold = rand_range(-10.0, 0.0);  // disabled range
+		double mean_res  = rand_range(1e-6, 1.0);
+
+		if (outlier_gate_fires(rms, threshold, mean_res)) {
+			fprintf(stderr, "DisabledWhenThresholdZero FAILED (seed=%u, trial=%d): "
+					"rms=%.4f threshold=%.4f mean_res=%.4f\n", seed, trial, rms, threshold, mean_res);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 2: Gate is disabled when mean_res <= 0 (cold start: no fleet baseline yet)
+TEST(OutlierGate, DisabledOnColdStart) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double rms       = rand_range(0.0, 100.0);
+		double threshold = rand_range(1.0, 10.0);   // enabled
+		double mean_res  = 0.0;                       // cold start
+
+		if (outlier_gate_fires(rms, threshold, mean_res)) {
+			fprintf(stderr, "DisabledOnColdStart FAILED (seed=%u, trial=%d): "
+					"rms=%.4f threshold=%.4f\n", seed, trial, rms, threshold);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 3: Gate fires when rms > threshold * mean_res
+TEST(OutlierGate, FiresAboveThreshold) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double threshold = rand_range(1.0, 10.0);
+		double mean_res  = rand_range(1e-4, 0.1);
+		// rms strictly above the trigger point
+		double rms = threshold * mean_res + rand_range(1e-6, 0.5);
+
+		if (!outlier_gate_fires(rms, threshold, mean_res)) {
+			fprintf(stderr, "FiresAboveThreshold FAILED (seed=%u, trial=%d): "
+					"rms=%.6f threshold=%.4f mean_res=%.6f trigger=%.6f\n",
+					seed, trial, rms, threshold, mean_res, threshold * mean_res);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 4: Gate does NOT fire when rms <= threshold * mean_res
+TEST(OutlierGate, DoesNotFireBelowThreshold) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double threshold = rand_range(1.0, 10.0);
+		double mean_res  = rand_range(1e-4, 0.1);
+		// rms strictly below the trigger point
+		double rms = rand_range(0.0, threshold * mean_res - 1e-8);
+		if (rms < 0) rms = 0;
+
+		if (outlier_gate_fires(rms, threshold, mean_res)) {
+			fprintf(stderr, "DoesNotFireBelowThreshold FAILED (seed=%u, trial=%d): "
+					"rms=%.6f threshold=%.4f mean_res=%.6f trigger=%.6f\n",
+					seed, trial, rms, threshold, mean_res, threshold * mean_res);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 5: RMS is non-negative and zero only for all-zero innovations
+TEST(OutlierGate, RmsNonNegativeAndZeroOnlyWhenAllZero) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	// All-zero case
+	{
+		double y[8] = {0};
+		double rms = compute_rms(y, 8);
+		if (rms != 0.0) {
+			fprintf(stderr, "RmsNonNegativeAndZeroOnlyWhenAllZero FAILED: all-zero rms=%.10f\n", rms);
+			return -1;
+		}
+	}
+
+	for (int trial = 0; trial < 10000; trial++) {
+		int cnt = 1 + rand() % 32;
+		double y[32];
+		bool all_zero = true;
+		for (int i = 0; i < cnt; i++) {
+			y[i] = rand_range(-1.0, 1.0);
+			if (y[i] != 0.0) all_zero = false;
+		}
+		double rms = compute_rms(y, cnt);
+		if (rms < 0) {
+			fprintf(stderr, "RmsNonNegativeAndZeroOnlyWhenAllZero FAILED: rms=%.6f < 0\n", rms);
+			return -1;
+		}
+		if (!all_zero && rms == 0.0) {
+			fprintf(stderr, "RmsNonNegativeAndZeroOnlyWhenAllZero FAILED: non-zero y but rms=0\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 6: Single large innovation (reflection scenario) triggers the gate.
+// A reflection might produce one sensor innovation >> normal; compute_rms
+// should be above a reasonable threshold even for a single outlier sensor.
+TEST(OutlierGate, SingleLargeInnovationTriggers) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	int triggered = 0;
+
+	for (int trial = 0; trial < 1000; trial++) {
+		int cnt = 8 + rand() % 24;          // 8-32 sensors
+		double mean_res = rand_range(0.005, 0.02);  // typical fleet residual
+		double threshold = 5.0;
+
+		// All sensors normal except one reflection spike
+		double y[32];
+		for (int i = 0; i < cnt; i++)
+			y[i] = rand_range(-mean_res, mean_res);
+		int spike_sensor = rand() % cnt;
+		// Reflection: 20-100× the normal noise
+		y[spike_sensor] = mean_res * rand_range(20.0, 100.0);
+
+		double rms = compute_rms(y, cnt);
+		if (outlier_gate_fires(rms, threshold, mean_res)) {
+			triggered++;
+		}
+	}
+
+	// A 20-100× spike on any single sensor in a batch of 8-32 should be detectable
+	if (triggered < 800) {
+		fprintf(stderr, "SingleLargeInnovationTriggers FAILED: only %d/1000 triggered\n", triggered);
+		return -1;
+	}
+	return 0;
+}