Add per-LH adaptive R to Kalman tracker (TE-PROC-038)

Scale each LH observation noise R by its residual relative to the fleet
mean, floored at 1.0. Miscalibrated LHs get less Kalman weight without
affecting well-calibrated ones. Populates the previously-stub fields
light_residuals[lh], lightcap_error_by_lh, and lightcap_count_by_lh.

Adds 6 AdaptiveR property tests in residual_cascade_props.c.

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

Author: Mark Stalzer

docs/specs/tracking-engine-specs.md

@@ -38,6 +38,7 @@ Prefix: **TE**
 - [x] **TE-PROC-035**: The system shall update each lightcap measurement's noise covariance R adaptively from observed residuals: `R_k = 0.3×R_{k-1} + 0.7×(residual² + H×P×H^T)`.
 - [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.
 
 ## Kalman Lighthouse Tracker
 

src/survive_kalman_tracker.c

@@ -448,12 +448,22 @@ void survive_kalman_tracker_integrate_saved_light(SurviveKalmanTracker *tracker,
 			};
 
 			SurviveObject *so = tracker->so;
-			FLT light_var = tracker->light_var;
 
-			SV_DATA_LOG("light_var", &light_var, 1);
+			// @spec TE-PROC-038
+			// Adaptive R: scale observation noise by per-LH residual history.
+			// A lighthouse with residuals above the fleet mean gets a larger R
+			// (less Kalman weight); equal residuals leave light_var unchanged.
+			FLT base_var = tracker->light_var;
+			FLT mean_res = tracker->light_residuals_all;
+			FLT lh_res   = tracker->light_residuals[lh];
+			FLT lh_var   = (mean_res > 0 && lh_res > 0)
+				? base_var * linmath_max(1.0, lh_res / mean_res)
+				: base_var;
+
+			SV_DATA_LOG("light_var", &lh_var, 1);
 			FLT light_vars[32] = {0};
 			for (int i = 0; i < 32; i++)
-				light_vars[i] = light_var;
+				light_vars[i] = lh_var;
 			CnMat R = cnVec(Z.rows, light_vars);
 
 			tracker->datalog_tag = "light_data";
@@ -466,16 +476,25 @@ void survive_kalman_tracker_integrate_saved_light(SurviveKalmanTracker *tracker,
 
 			tracker->last_light_time = time;
 
+			FLT lh_rtn;
 			if(useJointModel && tracker->joint_lightcap_ratio < ratio) {
 				tracker->joint_model.ks[0] = &ctx->bsd[lh].tracker->model;
 				tracker->joint_model.ks[1] = &ctx->bsd[lh].tracker->bsd_model;
-				rtn += cnkalman_meas_model_predict_update(time, &tracker->joint_model, &cbctx, &Z, &R);
+				lh_rtn = cnkalman_meas_model_predict_update(time, &tracker->joint_model, &cbctx, &Z, &R);
+				rtn += lh_rtn;
 				tracker->stats.joint_model_sensor_cnt_sum += cnt;
 				survive_kalman_lighthouse_report(ctx->bsd[lh].tracker);
 			} else {
-				rtn += cnkalman_meas_model_predict_update(time, &tracker->lightcap_model, &cbctx, &Z, &R);
+				lh_rtn = cnkalman_meas_model_predict_update(time, &tracker->lightcap_model, &cbctx, &Z, &R);
+				rtn += lh_rtn;
 				tracker->stats.lightcap_model_sensor_cnt_sum += cnt;
 			}
+
+			// Update per-LH residual EWMA (feeds adaptive R on subsequent frames)
+			tracker->light_residuals[lh] *= .9;
+			tracker->light_residuals[lh] += .1 * lh_rtn;
+			tracker->stats.lightcap_error_by_lh[lh] += lh_rtn;
+			tracker->stats.lightcap_count_by_lh[lh]++;
 		}
 
 		tracker->datalog_tag = 0;

src/test_cases/residual_cascade_props.c

@@ -774,3 +774,193 @@ TEST(ResidualCascade, SingleOutlierCanTriggerCascade) {
 	}
 	return 0;
 }
+
+// ── 8. Adaptive R (per-LH light_var scaling) Properties ─────────
+//
+// These test the invariants of the per-LH adaptive R formula introduced in
+// survive_kalman_tracker.c (TE-PROC-040):
+//
+//   lh_var = (mean_res > 0 && lh_res > 0)
+//              ? base_var * max(1.0, lh_res / mean_res)
+//              : base_var;
+//
+// The formula must satisfy:
+//   1. lh_var >= base_var always (never reduces below baseline)
+//   2. When lh_res == mean_res, lh_var == base_var (equal residuals → no change)
+//   3. When lh_res > mean_res, lh_var > base_var (biased LH gets penalized)
+//   4. Cold start (mean_res=0 or lh_res=0) → lh_var == base_var (safe fallback)
+//   5. lh_var is monotonically non-decreasing in lh_res
+
+static double adaptive_lh_var(double base_var, double mean_res, double lh_res) {
+	if (mean_res <= 0 || lh_res <= 0)
+		return base_var;
+	double ratio = lh_res / mean_res;
+	return base_var * (ratio > 1.0 ? ratio : 1.0);
+}
+
+// Property 1: lh_var is always >= base_var
+TEST(AdaptiveR, AlwaysAtLeastBaseVar) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double base_var = rand_range(1e-4, 1.0);
+		double mean_res = rand_range(0.0, 1.0);
+		double lh_res   = rand_range(0.0, 1.0);
+
+		double lh_var = adaptive_lh_var(base_var, mean_res, lh_res);
+
+		if (lh_var < base_var - 1e-12) {
+			fprintf(stderr, "AlwaysAtLeastBaseVar FAILED (seed=%u, trial=%d)\n", seed, trial);
+			fprintf(stderr, "  base_var=%.6f mean_res=%.6f lh_res=%.6f -> lh_var=%.6f\n",
+					base_var, mean_res, lh_res, lh_var);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 2: When lh_res == mean_res (and both > 0), lh_var == base_var
+TEST(AdaptiveR, EqualResidualIsIdentity) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double base_var = rand_range(1e-4, 1.0);
+		double res = rand_range(1e-6, 1.0);  // same for both mean and lh
+
+		double lh_var = adaptive_lh_var(base_var, res, res);
+
+		if (fabs(lh_var - base_var) > 1e-10) {
+			fprintf(stderr, "EqualResidualIsIdentity FAILED (seed=%u, trial=%d)\n", seed, trial);
+			fprintf(stderr, "  base_var=%.10f res=%.10f -> lh_var=%.10f (diff=%.2e)\n",
+					base_var, res, lh_var, fabs(lh_var - base_var));
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 3: lh_res > mean_res → lh_var > base_var
+TEST(AdaptiveR, HighResidualInflatesVar) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double base_var = rand_range(1e-4, 1.0);
+		double mean_res = rand_range(1e-6, 0.5);
+		// lh_res strictly greater than mean_res
+		double lh_res = mean_res + rand_range(1e-6, 0.5);
+
+		double lh_var = adaptive_lh_var(base_var, mean_res, lh_res);
+
+		if (lh_var <= base_var) {
+			fprintf(stderr, "HighResidualInflatesVar FAILED (seed=%u, trial=%d)\n", seed, trial);
+			fprintf(stderr, "  base_var=%.6f mean_res=%.6f lh_res=%.6f -> lh_var=%.6f\n",
+					base_var, mean_res, lh_res, lh_var);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 4: Cold start (mean_res=0 or lh_res=0) → safe fallback to base_var
+TEST(AdaptiveR, ColdStartFallsBackToBaseVar) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double base_var = rand_range(1e-4, 1.0);
+		double other    = rand_range(0.0, 1.0);
+
+		// Case A: mean_res = 0
+		double lh_var_a = adaptive_lh_var(base_var, 0.0, other);
+		if (fabs(lh_var_a - base_var) > 1e-12) {
+			fprintf(stderr, "ColdStartFallsBackToBaseVar FAILED case A (seed=%u, trial=%d)\n", seed, trial);
+			fprintf(stderr, "  base_var=%.6f mean_res=0.0 lh_res=%.6f -> lh_var=%.6f\n",
+					base_var, other, lh_var_a);
+			return -1;
+		}
+
+		// Case B: lh_res = 0
+		double lh_var_b = adaptive_lh_var(base_var, other, 0.0);
+		if (fabs(lh_var_b - base_var) > 1e-12) {
+			fprintf(stderr, "ColdStartFallsBackToBaseVar FAILED case B (seed=%u, trial=%d)\n", seed, trial);
+			fprintf(stderr, "  base_var=%.6f mean_res=%.6f lh_res=0.0 -> lh_var=%.6f\n",
+					base_var, other, lh_var_b);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 5: lh_var is monotonically non-decreasing in lh_res
+// (more residual error → equal or more variance, never less)
+TEST(AdaptiveR, MonotonicInLhResidual) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 10000; trial++) {
+		double base_var = rand_range(1e-4, 1.0);
+		double mean_res = rand_range(1e-6, 0.5);
+		double lh_res_lo = rand_range(1e-6, 0.5);
+		double lh_res_hi = lh_res_lo + rand_range(1e-6, 0.5);
+
+		double lh_var_lo = adaptive_lh_var(base_var, mean_res, lh_res_lo);
+		double lh_var_hi = adaptive_lh_var(base_var, mean_res, lh_res_hi);
+
+		if (lh_var_hi < lh_var_lo - 1e-12) {
+			fprintf(stderr, "MonotonicInLhResidual FAILED (seed=%u, trial=%d)\n", seed, trial);
+			fprintf(stderr, "  base_var=%.6f mean_res=%.6f\n", base_var, mean_res);
+			fprintf(stderr, "  lh_res_lo=%.6f -> lh_var=%.6f\n", lh_res_lo, lh_var_lo);
+			fprintf(stderr, "  lh_res_hi=%.6f -> lh_var=%.6f\n", lh_res_hi, lh_var_hi);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+// Property 6: Per-LH EWMA converges independently for each LH slot.
+// Each light_residuals[lh] slot uses the same EMA formula as light_residuals_all.
+// This test simulates two LHs with different steady-state residuals and verifies
+// that each EWMA slot converges to its own value without cross-contamination.
+TEST(AdaptiveR, PerLhEWMAConvergesIndependently) {
+	unsigned seed = (unsigned)time(NULL);
+	srand(seed);
+
+	for (int trial = 0; trial < 1000; trial++) {
+		double lh0_steady = rand_range(0.001, 0.1);
+		double lh1_steady = rand_range(0.001, 0.1);
+
+		double ema0 = 0.0, ema1 = 0.0;
+
+		// Drive both EWMA slots with their respective steady inputs
+		for (int i = 0; i < 100; i++) {
+			ema0 = ema_update(ema0, lh0_steady);
+			ema1 = ema_update(ema1, lh1_steady);
+		}
+
+		// Both should have converged (0.9^100 ≈ 2.7e-5, negligible)
+		if (fabs(ema0 - lh0_steady) > 1e-3) {
+			fprintf(stderr, "PerLhEWMAConvergesIndependently FAILED (seed=%u, trial=%d): "
+					"ema0=%.6f target=%.6f\n", seed, trial, ema0, lh0_steady);
+			return -1;
+		}
+		if (fabs(ema1 - lh1_steady) > 1e-3) {
+			fprintf(stderr, "PerLhEWMAConvergesIndependently FAILED (seed=%u, trial=%d): "
+					"ema1=%.6f target=%.6f\n", seed, trial, ema1, lh1_steady);
+			return -1;
+		}
+
+		// Slots must not influence each other
+		double expected_ratio = lh1_steady / lh0_steady;
+		double actual_ratio   = ema1 / ema0;
+		if (fabs(actual_ratio - expected_ratio) > expected_ratio * 0.01 + 1e-6) {
+			fprintf(stderr, "PerLhEWMAConvergesIndependently FAILED cross-contamination "
+					"(seed=%u, trial=%d): ratio=%.6f expected=%.6f\n",
+					seed, trial, actual_ratio, expected_ratio);
+			return -1;
+		}
+	}
+	return 0;
+}