fix(fusion): harden ESKF GPS failure handling

soc/software/fusion/README.md

@@ -338,7 +338,8 @@ Bu nedenle sorumluluk iki katmana ayrilir:
   olup olmadigini izler ve gerekli recovery/reset kararini burada verir.
 
 Mevcut yalın recovery, son başarılı position fusion'dan sonra yedi saniyelik
-timeout dolmuşsa ve en az üç rejected position fix'i kendi arasında zamansal
+timeout dolmuşsa, platform harici rest bilgisi ve IMU/GNSS hareket vetolarıyla
+duragansa ve en az üç rejected position fix'i kendi arasında zamansal
 tutarlıysa yalnız position state ve position covariance'ini GNSS'e yeniden
 ankrajlar. Position cross-covariance'lari gecersiz korelasyon tasimamasi icin
 sifirlanir; velocity, attitude ve IMU bias state'leri korunur. Hard NIS gate
@@ -569,6 +570,13 @@ make profile
 - sekiz ayrı deterministic noise/timing seed'i;
 - 30 s boyunca 500 Hz stationary IMU ve gecikmiş 1 Hz GPS.
 
+`gps_failure_scenario_test.c`, gerçek `gps_live_5min_20260713.csv` kaydındaki
+3.297 Hz ölçüm hızı, 300 ms medyan periyot, 1.803 s maksimum boşluk ve yaklaşık
+8.6 m yatay RMS saçılım ölçeğini kullanır. Normal akışın yanında hareket
+sırasında 12 s tam kesinti, yüzde 35 rastgele kayıp ve 1.8 s boşluklar, gecikmiş
+ölçümlerin toplu dönüşü ve kesinti sonrasında tutarlı fakat 80/-50/30 m hatalı
+üç fix senaryoları regression testidir.
+
 ### Sentetik hareketli senaryo
 
 Hareketli senaryo 200 Hz IMU ile 60 s sürer. Ground-truth profil şöyledir:

soc/software/fusion/tests/Makefile

@@ -10,7 +10,7 @@ IMU_PROFILE_SOURCE := ../../gps_system/src/adapters/imu_fusion_profile.c
 
 .PHONY: all test gps-system-test scenario-logs sanitize profile benchmark clean
 
-all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test
+all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test
 
 standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE) $(GPS_EPOCH_SOURCE)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
@@ -39,7 +39,10 @@ gps_reacquisition_test: gps_reacquisition_test.c $(ESKF_SOURCES)
 platform_scenario_matrix_test: platform_scenario_matrix_test.c $(ESKF_SOURCES)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
 
-test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test
+gps_failure_scenario_test: gps_failure_scenario_test.c $(ESKF_SOURCES)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test
 	./standalone_gps_integrity_test
 	./eskf_motion_test
 	./fusion_rest_detector_test
@@ -49,6 +52,7 @@ test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test f
 	./async_rate_matrix_test
 	./gps_reacquisition_test
 	./platform_scenario_matrix_test
+	./gps_failure_scenario_test
 
 gps-system-test: standalone_gps_integrity_test
 	./standalone_gps_integrity_test
@@ -122,6 +126,12 @@ sanitize:
 		-o platform_scenario_matrix_test_san $(LDLIBS)
 	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
 		UBSAN_OPTIONS=halt_on_error=1 ./platform_scenario_matrix_test_san
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header gps_failure_scenario_test.c $(ESKF_SOURCES) \
+		-o gps_failure_scenario_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./gps_failure_scenario_test_san
 
 profile:
 	$(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \
@@ -136,4 +146,4 @@ benchmark: standalone_gps_integrity_test
 	./standalone_gps_integrity_test --bench-double 100000
 
 clean:
-	rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_rest_detector_test fusion_rest_detector_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gmon.out
+	rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_rest_detector_test fusion_rest_detector_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gps_failure_scenario_test gps_failure_scenario_test_san gmon.out

soc/software/fusion/tests/eskf_motion_test.c

@@ -119,6 +119,51 @@ static void test_zero_update_primitives(void)
 	}
 }
 
+static double quaternion_yaw(const double quaternion[4])
+{
+	return atan2(
+		2.0 * (quaternion[0] * quaternion[3] +
+		       quaternion[1] * quaternion[2]),
+		1.0 - 2.0 * (quaternion[2] * quaternion[2] +
+		             quaternion[3] * quaternion[3]));
+}
+
+static void test_external_rest_cannot_erase_constant_turn(void)
+{
+	fusion_engine_config_t config;
+	fusion_engine_t engine;
+	fusion_engine_default_config(&config, 1000000u);
+	CHECK(fusion_engine_init(&engine, &config),
+	      "constant-turn engine initialization");
+
+	for(unsigned int sample = 0u ; sample <= 8u * 52u ; sample++)
+	{
+		uint64_t ticks = ((uint64_t)sample * 1000000u + 26u) / 52u;
+		double time_s = (double)ticks / 1000000.0;
+		fusion_imu_measurement_t imu;
+		memset(&imu, 0, sizeof(imu));
+		imu.timestamp_ticks = ticks;
+		imu.accel_mps2[2] = config.eskf.gravity_mps2;
+		imu.gyro_rps[2] =
+			(time_s >= 3.0) ? 20.0 * TEST_PI / 180.0 : 0.0;
+		imu.temperature_c = 25.0;
+		/* Models a stale/incorrect external landed flag.  Inertial motion must
+		 * still veto ZUPT and zero-gyro fusion. */
+		imu.vehicle_at_rest = true;
+		CHECK(fusion_engine_push_imu(&engine, &imu),
+		      "constant-turn IMU accepted");
+	}
+	CHECK(engine.state == FUSION_ENGINE_RUNNING,
+	      "constant-turn engine remains running");
+	double yaw_deg = quaternion_yaw(
+		engine.current.filter.body_to_nav_quaternion) * 180.0 / TEST_PI;
+	CHECK(fabs(yaw_deg - 100.0) < 1.0,
+	      "stale external rest flag cannot erase a real constant turn");
+	CHECK(fabs(engine.current.filter.gyro_bias_rps[2]) <
+	      0.5 * TEST_PI / 180.0,
+	      "constant angular rate is not learned as gyro bias");
+}
+
 static void test_gravity_alignment(void)
 {
 	eskf_t filter;
@@ -747,6 +792,7 @@ int main(void)
 {
 	test_stationary_propagation();
 	test_zero_update_primitives();
+	test_external_rest_cannot_erase_constant_turn();
 	test_gravity_alignment();
 	test_position_nis_gate();
 	test_motion_transitions();

soc/software/fusion/tests/gps_failure_scenario_test.c

@@ -0,0 +1,436 @@
+#include "fusion/fusion_engine.h"
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#define TIMEBASE_HZ 1000000u
+#define IMU_RATE_HZ 52u
+#define GPS_PERIOD_TICKS 300000u
+#define MAX_GPS_EVENTS 256u
+#define PI 3.14159265358979323846
+
+/*
+ * Timing/noise scale comes from logs/gps_live_5min_20260713.csv:
+ * 988 fixes in 299.697 s (3.297 Hz), 300 ms median interval, 1.803 s maximum
+ * interval, and about 8.6 m stationary horizontal RMS scatter.  The 6 m
+ * per-axis covariance below reproduces that horizontal RMS scale.
+ */
+typedef enum {
+	SCENARIO_NORMAL = 0,
+	SCENARIO_MOVING_OUTAGE,
+	SCENARIO_INTERMITTENT,
+	SCENARIO_BURST_RETURN,
+	SCENARIO_BAD_RESTART
+} scenario_kind_t;
+
+typedef struct {
+	fusion_position_measurement_t measurement;
+	uint64_t delivery_ticks;
+} gps_event_t;
+
+typedef struct {
+	const char* name;
+	double horizontal_rmse_m;
+	double maximum_outage_error_m;
+	double error_at_outage_end_m;
+	double final_horizontal_error_m;
+	double maximum_gps_state_jump_m;
+	double speed_at_outage_start_mps;
+	double speed_at_outage_end_mps;
+	double horizontal_sigma_at_outage_end_m;
+	uint64_t gps_accepted;
+	uint64_t gps_rejected;
+	uint64_t gps_recoveries;
+	uint64_t stale_rejected;
+	uint64_t replay_count;
+	unsigned int generated;
+	unsigned int dropped;
+	bool finite;
+} scenario_metrics_t;
+
+static uint32_t random_state;
+
+static double uniform_random(void)
+{
+	random_state = 1664525u * random_state + 1013904223u;
+	return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) /
+	       16777216.0;
+}
+
+static double gaussian_random(void)
+{
+	double first = fmax(uniform_random(), 1.0e-12);
+	double second = uniform_random();
+	return sqrt(-2.0 * log(first)) * cos(2.0 * PI * second);
+}
+
+static void truth_at(double time_s, double position[3],
+	                 double velocity[3], double acceleration[3])
+{
+	memset(position, 0, 3u * sizeof(double));
+	memset(velocity, 0, 3u * sizeof(double));
+	memset(acceleration, 0, 3u * sizeof(double));
+	if(time_s < 3.0)
+	{
+		return;
+	}
+	if(time_s < 5.0)
+	{
+		double elapsed = time_s - 3.0;
+		acceleration[0] = 1.5;
+		velocity[0] = 1.5 * elapsed;
+		position[0] = 0.75 * elapsed * elapsed;
+		return;
+	}
+	if(time_s < 50.0)
+	{
+		velocity[0] = 3.0;
+		position[0] = 3.0 + 3.0 * (time_s - 5.0);
+		return;
+	}
+	if(time_s < 52.0)
+	{
+		double elapsed = time_s - 50.0;
+		acceleration[0] = -1.5;
+		velocity[0] = 3.0 - 1.5 * elapsed;
+		position[0] = 138.0 + 3.0 * elapsed - 0.75 * elapsed * elapsed;
+		return;
+	}
+	position[0] = 141.0;
+}
+
+static fusion_imu_measurement_t make_imu(
+	uint64_t ticks, unsigned int sequence, double gravity_mps2)
+{
+	double time_s = (double)ticks / (double)TIMEBASE_HZ;
+	double position[3];
+	double velocity[3];
+	double acceleration[3];
+	truth_at(time_s, position, velocity, acceleration);
+	(void)position;
+	(void)velocity;
+
+	fusion_imu_measurement_t imu;
+	memset(&imu, 0, sizeof(imu));
+	imu.timestamp_ticks = ticks;
+	imu.accel_mps2[0] = acceleration[0] + 0.030;
+	imu.accel_mps2[1] = -0.020;
+	imu.accel_mps2[2] = gravity_mps2 + 0.050;
+	imu.gyro_rps[0] = 0.0008;
+	imu.gyro_rps[1] = -0.0005;
+	imu.gyro_rps[2] = 0.0006;
+	if(time_s >= 3.0)
+	{
+		/* Small temperature-like residual drift after startup calibration. */
+		imu.accel_mps2[0] += 0.012 * (time_s - 3.0) / 62.0;
+	}
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		double phase = 0.37 * (double)sequence + 1.7 * (double)axis;
+		imu.accel_mps2[axis] += 0.025 * sin(phase);
+		imu.gyro_rps[axis] += 0.0007 * cos(1.31 * phase);
+	}
+	imu.temperature_c = 25.0 + 0.02 * time_s;
+	imu.sequence = sequence;
+	imu.vehicle_at_rest = (time_s < 2.9) || (time_s >= 52.5);
+	return imu;
+}
+
+static fusion_position_measurement_t make_gps(
+	uint64_t capture_ticks, bool bad_restart_fix)
+{
+	double position[3];
+	double velocity[3];
+	double acceleration[3];
+	truth_at((double)capture_ticks / (double)TIMEBASE_HZ,
+	         position, velocity, acceleration);
+	(void)velocity;
+	(void)acceleration;
+
+	fusion_position_measurement_t gps;
+	memset(&gps, 0, sizeof(gps));
+	gps.timestamp_ticks = capture_ticks;
+	gps.position_enu_m[0] = position[0] + 6.0 * gaussian_random();
+	gps.position_enu_m[1] = position[1] + 6.0 * gaussian_random();
+	gps.position_enu_m[2] = position[2] + 10.0 * gaussian_random();
+	if(bad_restart_fix)
+	{
+		gps.position_enu_m[0] += 80.0;
+		gps.position_enu_m[1] -= 50.0;
+		gps.position_enu_m[2] += 30.0;
+	}
+	gps.covariance_enu_m2[0] = 36.0;
+	gps.covariance_enu_m2[4] = 36.0;
+	gps.covariance_enu_m2[8] = 100.0;
+	gps.source_quality = 80.0;
+	gps.source_valid = true;
+	return gps;
+}
+
+static bool event_should_drop(scenario_kind_t kind, double capture_s)
+{
+	if((kind == SCENARIO_MOVING_OUTAGE) &&
+	   (capture_s >= 15.0) && (capture_s < 27.0))
+	{
+		return true;
+	}
+	if((kind == SCENARIO_BAD_RESTART) &&
+	   (capture_s >= 15.0) && (capture_s < 23.1))
+	{
+		return true;
+	}
+	if(kind == SCENARIO_INTERMITTENT)
+	{
+		bool natural_long_gap =
+			((capture_s >= 12.0) && (capture_s < 13.8)) ||
+			((capture_s >= 31.0) && (capture_s < 32.8));
+		return natural_long_gap || (uniform_random() < 0.35);
+	}
+	return false;
+}
+
+static unsigned int build_events(
+	scenario_kind_t kind, gps_event_t events[MAX_GPS_EVENTS],
+	unsigned int* dropped_out)
+{
+	unsigned int count = 0u;
+	unsigned int dropped = 0u;
+	uint64_t previous_delivery = 0u;
+	for(uint64_t capture = 3300000u ; capture <= 64000000u ;
+	    capture += GPS_PERIOD_TICKS)
+	{
+		double capture_s = (double)capture / (double)TIMEBASE_HZ;
+		if(event_should_drop(kind, capture_s))
+		{
+			dropped++;
+			continue;
+		}
+		if(count >= MAX_GPS_EVENTS)
+		{
+			break;
+		}
+		bool bad_restart_fix = (kind == SCENARIO_BAD_RESTART) &&
+		                       (capture_s >= 23.1) && (capture_s < 24.0);
+		events[count].measurement = make_gps(capture, bad_restart_fix);
+		uint64_t delay = 120000u + (uint64_t)(250000.0 * uniform_random());
+		uint64_t delivery = capture + delay;
+		if((kind == SCENARIO_BURST_RETURN) &&
+		   (capture_s >= 15.0) && (capture_s < 20.5))
+		{
+			delivery = 20700000u;
+		}
+		/* A receiver normally publishes epochs in capture order even when its
+		 * processing latency jitters. Preserve that property here. */
+		if(delivery < previous_delivery)
+		{
+			delivery = previous_delivery;
+		}
+		events[count].delivery_ticks = delivery;
+		previous_delivery = delivery;
+		count++;
+	}
+	*dropped_out = dropped;
+	return count;
+}
+
+static double horizontal_error(const fusion_engine_t* engine, uint64_t ticks)
+{
+	double truth_position[3];
+	double truth_velocity[3];
+	double truth_acceleration[3];
+	truth_at((double)ticks / (double)TIMEBASE_HZ,
+	         truth_position, truth_velocity, truth_acceleration);
+	(void)truth_velocity;
+	(void)truth_acceleration;
+	return hypot(engine->current.filter.position_m[0] - truth_position[0],
+	             engine->current.filter.position_m[1] - truth_position[1]);
+}
+
+static scenario_metrics_t run_scenario(
+	scenario_kind_t kind, const char* name, uint32_t seed)
+{
+	static fusion_engine_t engine;
+	static gps_event_t events[MAX_GPS_EVENTS];
+	fusion_engine_config_t config;
+	scenario_metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+	metrics.name = name;
+	random_state = seed;
+	unsigned int dropped = 0u;
+	unsigned int event_count = build_events(kind, events, &dropped);
+	metrics.generated = event_count;
+	metrics.dropped = dropped;
+
+	fusion_engine_default_config(&config, TIMEBASE_HZ);
+	if(!fusion_engine_init(&engine, &config))
+	{
+		return metrics;
+	}
+	fusion_position_measurement_t origin;
+	memset(&origin, 0, sizeof(origin));
+	origin.timestamp_ticks = 1u;
+	origin.covariance_enu_m2[0] = 36.0;
+	origin.covariance_enu_m2[4] = 36.0;
+	origin.covariance_enu_m2[8] = 100.0;
+	origin.source_quality = 100.0;
+	origin.source_valid = true;
+	if(!fusion_engine_push_position(&engine, &origin, NULL, NULL))
+	{
+		return metrics;
+	}
+
+	unsigned int event_head = 0u;
+	double squared_error_sum = 0.0;
+	unsigned int error_count = 0u;
+	unsigned int imu_count = 65u * IMU_RATE_HZ;
+	for(unsigned int sample = 0u ; sample <= imu_count ; sample++)
+	{
+		uint64_t ticks =
+			((uint64_t)sample * TIMEBASE_HZ + IMU_RATE_HZ / 2u) / IMU_RATE_HZ;
+		fusion_imu_measurement_t imu = make_imu(
+			ticks, sample, config.eskf.gravity_mps2);
+		if(!fusion_engine_push_imu(&engine, &imu))
+		{
+			return metrics;
+		}
+
+		while((event_head < event_count) &&
+		      (events[event_head].delivery_ticks <= ticks))
+		{
+			double before[3];
+			memcpy(before, engine.current.filter.position_m, sizeof(before));
+			(void)fusion_engine_push_position(
+				&engine, &events[event_head].measurement, NULL, NULL);
+			double jump = hypot(
+				engine.current.filter.position_m[0] - before[0],
+				engine.current.filter.position_m[1] - before[1]);
+			if(jump > metrics.maximum_gps_state_jump_m)
+			{
+				metrics.maximum_gps_state_jump_m = jump;
+			}
+			event_head++;
+		}
+
+		if(engine.state != FUSION_ENGINE_RUNNING)
+		{
+			continue;
+		}
+		double time_s = (double)ticks / (double)TIMEBASE_HZ;
+		double error = horizontal_error(&engine, ticks);
+		if(time_s >= 3.0)
+		{
+			squared_error_sum += error * error;
+			error_count++;
+		}
+		if((time_s >= 15.0) && (time_s <= 27.0) &&
+		   (error > metrics.maximum_outage_error_m))
+		{
+			metrics.maximum_outage_error_m = error;
+		}
+		if(fabs(time_s - 27.0) < 0.5 / (double)IMU_RATE_HZ)
+		{
+			metrics.error_at_outage_end_m = error;
+			metrics.speed_at_outage_end_mps =
+				hypot(engine.current.filter.velocity_mps[0],
+				      engine.current.filter.velocity_mps[1]);
+			metrics.horizontal_sigma_at_outage_end_m = sqrt(
+				engine.current.filter.covariance[0][0] +
+				engine.current.filter.covariance[1][1]);
+		}
+		if(fabs(time_s - 15.0) < 0.5 / (double)IMU_RATE_HZ)
+		{
+			metrics.speed_at_outage_start_mps =
+				hypot(engine.current.filter.velocity_mps[0],
+				      engine.current.filter.velocity_mps[1]);
+		}
+	}
+
+	metrics.horizontal_rmse_m =
+		(error_count != 0u) ? sqrt(squared_error_sum / (double)error_count) : NAN;
+	metrics.final_horizontal_error_m =
+		horizontal_error(&engine, 65u * TIMEBASE_HZ);
+	metrics.gps_accepted = engine.diagnostics.position_accepted;
+	metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
+	metrics.gps_recoveries = engine.diagnostics.position_recovery_count;
+	metrics.stale_rejected = engine.diagnostics.position_stale_rejected;
+	metrics.replay_count = engine.diagnostics.replay_count;
+	metrics.finite = (engine.state == FUSION_ENGINE_RUNNING) &&
+	                 eskf_state_is_finite(&engine.current.filter) &&
+	                 isfinite(metrics.horizontal_rmse_m) &&
+	                 isfinite(metrics.final_horizontal_error_m);
+	return metrics;
+}
+
+static void print_metrics(const scenario_metrics_t* metrics)
+{
+	printf("%-18s fixes=%3u drop=%2u accepted/rejected=%3llu/%2llu "
+	       "recovery=%llu stale=%llu rmse=%5.2fm outage-max/end=%5.2f/%5.2fm "
+	       "speed15/27=%4.2f/%4.2f sigma27=%5.1fm jump=%5.2fm final=%5.2fm finite=%u\n",
+	       metrics->name, metrics->generated, metrics->dropped,
+	       (unsigned long long)metrics->gps_accepted,
+	       (unsigned long long)metrics->gps_rejected,
+	       (unsigned long long)metrics->gps_recoveries,
+	       (unsigned long long)metrics->stale_rejected,
+	       metrics->horizontal_rmse_m,
+	       metrics->maximum_outage_error_m,
+	       metrics->error_at_outage_end_m,
+	       metrics->speed_at_outage_start_mps,
+	       metrics->speed_at_outage_end_mps,
+	       metrics->horizontal_sigma_at_outage_end_m,
+	       metrics->maximum_gps_state_jump_m,
+	       metrics->final_horizontal_error_m,
+	       metrics->finite ? 1u : 0u);
+}
+
+int main(void)
+{
+	scenario_metrics_t normal = run_scenario(
+		SCENARIO_NORMAL, "normal", 0x10203040u);
+	scenario_metrics_t outage = run_scenario(
+		SCENARIO_MOVING_OUTAGE, "12s-moving-outage", 0x20304050u);
+	scenario_metrics_t intermittent = run_scenario(
+		SCENARIO_INTERMITTENT, "intermittent", 0x30405060u);
+	scenario_metrics_t burst = run_scenario(
+		SCENARIO_BURST_RETURN, "burst-return", 0x40506070u);
+	scenario_metrics_t restart = run_scenario(
+		SCENARIO_BAD_RESTART, "bad-restart", 0x50607080u);
+
+	puts("Real-log-scaled GNSS failure scenarios: 3.3 Hz, 6 m horizontal / 10 m vertical sigma");
+	print_metrics(&normal);
+	print_metrics(&outage);
+	print_metrics(&intermittent);
+	print_metrics(&burst);
+	print_metrics(&restart);
+
+	bool common_ok =
+		normal.finite && outage.finite && intermittent.finite && burst.finite &&
+		restart.finite &&
+		(normal.horizontal_rmse_m < 5.0) &&
+		/* Position-only dead reckoning is allowed to drift, but the error must
+		 * remain statistically covered and recover once GNSS returns. */
+		(outage.maximum_outage_error_m <
+		 2.0 * outage.horizontal_sigma_at_outage_end_m) &&
+		(outage.horizontal_sigma_at_outage_end_m > 10.0) &&
+		(outage.final_horizontal_error_m < 5.0) &&
+		(intermittent.final_horizontal_error_m < 6.0) &&
+		(burst.stale_rejected == 0u) &&
+		(burst.replay_count > 0u);
+	bool restart_safe =
+		(restart.gps_recoveries == 0u) &&
+		(restart.gps_rejected >= 3u) &&
+		(restart.maximum_gps_state_jump_m < 50.0) &&
+		(restart.final_horizontal_error_m < 6.0);
+	if(!common_ok || !restart_safe)
+	{
+		fprintf(stderr,
+		        "FAIL: GNSS outage/reacquisition safety regression "
+		        "(common=%u restart-safe=%u)\n",
+		        common_ok ? 1u : 0u, restart_safe ? 1u : 0u);
+		return 1;
+	}
+	puts("PASS: realistic GNSS loss, intermittent delivery and reacquisition stayed bounded.");
+	return 0;
+}

soc/software/gps_system/README.md

@@ -165,7 +165,8 @@ uzakta kalirsa, yalniz hard NIS rejection kullanmak filtreyi yanlis baslangica
 kilitler. Fusion engine bu nedenle ardisik reddedilen position fix'lerinin
 zamansal surekliligini izler. Son başarılı fusion'dan sonra yedi saniye timeout
 dolmuşsa ve en az uc fix birbirinin devamı
-ise yalniz ESKF konumu ve konum kovaryansi son olcume yeniden ankrajlanir;
+ise ve platform IMU/GNSS hareket vetolariyla duragan kabul ediliyorsa yalniz
+ESKF konumu ve konum kovaryansi son olcume yeniden ankrajlanir;
 attitude, hiz ve IMU bias state'leri korunur, gecersiz hale gelen position
 cross-covariance'lari sifirlanir. Tek aykiri fix bu yolu tetiklemez. Bu davranis
 100/200/1000 ms GPS oranlarinda `gps_reacquisition_test.c` ile dogrulanir ve

soc/software/gps_system/src/fusion/eskf.c

@@ -390,11 +390,16 @@ static bool eskf_propagate_step(eskf_t* filter,
 		return false;
 	}
 
-	double system[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
 	for(unsigned int i = 0 ; i < 3u ; i++)
 	{
-		system[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
-		system[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -1.0;
+		transition[i][i] = 1.0;
+		transition[ESKF_VELOCITY_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
+		transition[ESKF_ATTITUDE_INDEX + i][ESKF_ATTITUDE_INDEX + i] = 1.0;
+		transition[ESKF_ACCEL_BIAS_INDEX + i][ESKF_ACCEL_BIAS_INDEX + i] = 1.0;
+		transition[ESKF_GYRO_BIAS_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = 1.0;
+		transition[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = dt_s;
+		transition[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -dt_s;
 	}
 	double force_skew[3][3];
 	double rate_skew[3][3];
@@ -409,27 +414,16 @@ static bool eskf_propagate_step(eskf_t* filter,
 			{
 				rotated_skew += rotation[row][k] * force_skew[k][col];
 			}
-			system[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
-				-rotated_skew;
-			system[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
-				-rotation[row][col];
-			system[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
-				-rate_skew[row][col];
+			transition[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] +=
+				-rotated_skew * dt_s;
+			transition[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
+				-rotation[row][col] * dt_s;
+			transition[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] +=
+				-rate_skew[row][col] * dt_s;
 		}
 	}
 
-	double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
-	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
-	{
-		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
-		{
-			transition[row][col] = system[row][col] * dt_s;
-		}
-		transition[row][row] += 1.0;
-	}
-
 	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
-	double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
 		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
@@ -448,21 +442,18 @@ static bool eskf_propagate_step(eskf_t* filter,
 	}
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
-		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 		{
-			double value = intermediate[row][k];
-			if(value == 0.0)
-			{
-				continue;
-			}
-			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			double value = 0.0;
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
 			{
 				double coefficient = transition[col][k];
 				if(coefficient != 0.0)
 				{
-					propagated[row][col] += value * coefficient;
+					value += intermediate[row][k] * coefficient;
 				}
 			}
+			filter->covariance[row][col] = value;
 		}
 	}
 
@@ -477,17 +468,20 @@ static bool eskf_propagate_step(eskf_t* filter,
 				               square(config->accel_noise_density[axis]) *
 				               rotation[col][axis];
 			}
-			propagated[ESKF_VELOCITY_INDEX + row][ESKF_VELOCITY_INDEX + col] +=
+			filter->covariance[ESKF_VELOCITY_INDEX + row]
+			                  [ESKF_VELOCITY_INDEX + col] +=
 				accel_noise * dt_s;
 		}
-		propagated[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + row] +=
+		filter->covariance[ESKF_ATTITUDE_INDEX + row]
+		                  [ESKF_ATTITUDE_INDEX + row] +=
 			square(config->gyro_noise_density[row]) * dt_s;
-		propagated[ESKF_ACCEL_BIAS_INDEX + row][ESKF_ACCEL_BIAS_INDEX + row] +=
+		filter->covariance[ESKF_ACCEL_BIAS_INDEX + row]
+		                  [ESKF_ACCEL_BIAS_INDEX + row] +=
 			square(config->accel_bias_random_walk[row]) * dt_s;
-		propagated[ESKF_GYRO_BIAS_INDEX + row][ESKF_GYRO_BIAS_INDEX + row] +=
+		filter->covariance[ESKF_GYRO_BIAS_INDEX + row]
+		                  [ESKF_GYRO_BIAS_INDEX + row] +=
 			square(config->gyro_bias_random_walk[row]) * dt_s;
 	}
-	memcpy(filter->covariance, propagated, sizeof(propagated));
 	covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
 	filter->propagation_count++;
 	return eskf_state_is_finite(filter);
@@ -591,10 +585,10 @@ static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STAT
 	}
 
 	/* Right attitude-error reset after q <- q (x) Exp(delta_theta). */
-	double reset[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
-	for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
+	double attitude_reset[3][3] = {{0.0}};
+	for(unsigned int i = 0 ; i < 3u ; i++)
 	{
-		reset[i][i] = 1.0;
+		attitude_reset[i][i] = 1.0;
 	}
 	double attitude_skew[3][3];
 	skew_matrix(attitude_error, attitude_skew);
@@ -602,47 +596,48 @@ static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STAT
 	{
 		for(unsigned int col = 0 ; col < 3u ; col++)
 		{
-			reset[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] -=
-				0.5 * attitude_skew[row][col];
+			attitude_reset[row][col] -= 0.5 * attitude_skew[row][col];
 		}
 	}
-	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
-	double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
-	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+
+	/* G differs from identity only in the 3x3 attitude block.  Apply G*P
+	 * using three temporary rows, then apply G^T using three scalar columns.
+	 * This keeps the Joseph/reset path below the bare-metal stack budget. */
+	double attitude_rows[3][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0u ; row < 3u ; row++)
 	{
-		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+		for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
 		{
-			double coefficient = reset[row][k];
-			if(coefficient == 0.0)
+			for(unsigned int k = 0u ; k < 3u ; k++)
 			{
-				continue;
-			}
-			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
-			{
-				intermediate[row][col] += coefficient * filter->covariance[k][col];
+				attitude_rows[row][col] += attitude_reset[row][k] *
+					filter->covariance[ESKF_ATTITUDE_INDEX + k][col];
 			}
 		}
 	}
-	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	for(unsigned int row = 0u ; row < 3u ; row++)
 	{
-		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+		memcpy(filter->covariance[ESKF_ATTITUDE_INDEX + row],
+		       attitude_rows[row], sizeof(attitude_rows[row]));
+	}
+	for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		double attitude_columns[3] = {0.0, 0.0, 0.0};
+		for(unsigned int col = 0u ; col < 3u ; col++)
 		{
-			double value = intermediate[row][k];
-			if(value == 0.0)
+			for(unsigned int k = 0u ; k < 3u ; k++)
 			{
-				continue;
-			}
-			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
-			{
-				double coefficient = reset[col][k];
-				if(coefficient != 0.0)
-				{
-					reset_covariance[row][col] += value * coefficient;
-				}
+				attitude_columns[col] +=
+					filter->covariance[row][ESKF_ATTITUDE_INDEX + k] *
+					attitude_reset[col][k];
 			}
 		}
+		for(unsigned int col = 0u ; col < 3u ; col++)
+		{
+			filter->covariance[row][ESKF_ATTITUDE_INDEX + col] =
+				attitude_columns[col];
+		}
 	}
-	memcpy(filter->covariance, reset_covariance, sizeof(reset_covariance));
 	return true;
 }
 
@@ -719,76 +714,44 @@ static bool eskf_selected_update(eskf_t* filter,
 		}
 	}
 
-	double joseph_left[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
-	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
-	{
-		joseph_left[row][row] = 1.0;
-		for(unsigned int col = 0 ; col < 3u ; col++)
-		{
-			joseph_left[row][selected_state_index + col] -= gain[row][col];
-		}
-	}
+	/* Joseph form with A = I-KH.  H selects three direct state components, so
+	 * A*P can be formed without materializing a second 15x15 matrix. */
 	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
-	double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
-		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 		{
-			double coefficient = joseph_left[row][k];
-			if(coefficient == 0.0)
+			double value = filter->covariance[row][col];
+			for(unsigned int selected = 0u ; selected < 3u ; selected++)
 			{
-				continue;
-			}
-			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
-			{
-				intermediate[row][col] +=
-					coefficient * filter->covariance[k][col];
-			}
-		}
-	}
-	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
-	{
-		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
-		{
-			double value = intermediate[row][k];
-			if(value == 0.0)
-			{
-				continue;
-			}
-			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
-			{
-				double coefficient = joseph_left[col][k];
-				if(coefficient != 0.0)
-				{
-					updated_covariance[row][col] += value * coefficient;
-				}
-			}
-		}
-	}
-	double gain_times_covariance[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
-	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
-	{
-		for(unsigned int second = 0 ; second < 3u ; second++)
-		{
-			for(unsigned int first = 0 ; first < 3u ; first++)
-			{
-				gain_times_covariance[row][second] += gain[row][first] *
-					measurement_covariance[first * 3u + second];
+				value -= gain[row][selected] *
+					filter->covariance[selected_state_index + selected][col];
 			}
+			intermediate[row][col] = value;
 		}
 	}
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
 		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 		{
-			for(unsigned int second = 0 ; second < 3u ; second++)
+			double value = intermediate[row][col];
+			for(unsigned int selected = 0u ; selected < 3u ; selected++)
 			{
-				updated_covariance[row][col] +=
-					gain_times_covariance[row][second] * gain[col][second];
+				value -= intermediate[row][selected_state_index + selected] *
+					gain[col][selected];
 			}
+			for(unsigned int first = 0u ; first < 3u ; first++)
+			{
+				for(unsigned int second = 0u ; second < 3u ; second++)
+				{
+					value += gain[row][first] *
+						measurement_covariance[first * 3u + second] *
+						gain[col][second];
+				}
+			}
+			filter->covariance[row][col] = value;
 		}
 	}
-	memcpy(filter->covariance, updated_covariance, sizeof(updated_covariance));
 	if(!eskf_inject_error(filter, error))
 	{
 		return false;

soc/software/gps_system/src/fusion/fusion_engine.c

@@ -136,8 +136,8 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
 	config->gnss_moving_confirmations = 2u;
 	config->gnss_still_confirmations = 2u;
 	/* A hard innovation gate protects against isolated GNSS outliers, but it
-	 * must not permanently lock the filter to a bad startup position.  After a
-	 * full second of mutually continuous rejected fixes, re-anchor position
+	 * must not permanently lock a stationary filter to a bad startup position.
+	 * After a timeout of mutually continuous rejected fixes, re-anchor position
 	 * while preserving inertial velocity, attitude and bias states. */
 	config->position_recovery_timeout_s = 7.0;
 	config->position_recovery_max_speed_mps = 100.0;
@@ -301,6 +301,31 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 	                          const fusion_imu_measurement_t* measurement,
 	                          bool count_zero_updates);
 
+static bool runtime_allows_zero_velocity_update(
+	const fusion_engine_t* engine,
+	const fusion_runtime_t* runtime)
+{
+	(void)engine;
+	return runtime->vehicle_at_rest &&
+	       (runtime->gnss_evidence != FUSION_GNSS_MOTION_MOVING);
+}
+
+static bool runtime_allows_zero_gyro_update(
+	const fusion_engine_t* engine,
+	const fusion_runtime_t* runtime)
+{
+	return runtime_allows_zero_velocity_update(engine, runtime) &&
+	       (runtime->motion.last_gyro_norm_rps <=
+	        engine->config.motion.stationary_gyro_limit_rps);
+}
+
+static bool runtime_allows_position_recovery(
+	const fusion_engine_t* engine,
+	const fusion_runtime_t* runtime)
+{
+	return runtime_allows_zero_gyro_update(engine, runtime);
+}
+
 static void history_push(fusion_engine_t* engine,
 	                     const fusion_imu_measurement_t* measurement)
 {
@@ -415,9 +440,12 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 		runtime->gnss_still_count = 0u;
 	}
 	runtime->vehicle_at_rest = measurement->vehicle_at_rest;
-	bool at_rest_continuing = measurement->vehicle_at_rest &&
-	                          start_measurement.vehicle_at_rest;
-	if(measurement->vehicle_at_rest)
+	bool zero_velocity_allowed =
+		runtime_allows_zero_velocity_update(engine, runtime);
+	bool zero_gyro_allowed =
+		runtime_allows_zero_gyro_update(engine, runtime);
+	bool at_rest_continuing = start_measurement.vehicle_at_rest;
+	if(zero_gyro_allowed && at_rest_continuing)
 	{
 		for(unsigned int axis = 0u ; axis < 3u ; axis++)
 		{
@@ -435,36 +463,51 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 	}
 	uint64_t zero_period_ticks =
 		seconds_to_ticks(engine, engine->config.zero_update_period_s);
-	if(at_rest_continuing &&
+	if(at_rest_continuing && (zero_velocity_allowed || zero_gyro_allowed) &&
 	   ((measurement->timestamp_ticks - runtime->last_zero_update_ticks) >=
 	    zero_period_ticks))
 	{
-		double mean_gyro_rps[3];
-		if(!(runtime->zero_gyro_duration_s > 0.0))
+		bool update_applied = false;
+		if(zero_velocity_allowed)
 		{
-			return false;
+			if(!eskf_zero_velocity_update(
+				&runtime->filter, engine->config.zero_velocity_std_mps))
+			{
+				return false;
+			}
+			update_applied = true;
+			if(count_zero_updates)
+			{
+				engine->diagnostics.zero_velocity_update_count++;
+			}
 		}
-		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		if(zero_gyro_allowed && (runtime->zero_gyro_duration_s > 0.0))
 		{
-			mean_gyro_rps[axis] = runtime->zero_gyro_integral_rad[axis] /
-			                      runtime->zero_gyro_duration_s;
+			double mean_gyro_rps[3];
+			for(unsigned int axis = 0u ; axis < 3u ; axis++)
+			{
+				mean_gyro_rps[axis] = runtime->zero_gyro_integral_rad[axis] /
+				                      runtime->zero_gyro_duration_s;
+			}
+			if(!eskf_zero_angular_rate_update(
+				&runtime->filter, mean_gyro_rps,
+				engine->config.zero_gyro_std_rps))
+			{
+				return false;
+			}
+			update_applied = true;
+			if(count_zero_updates)
+			{
+				engine->diagnostics.zero_gyro_update_count++;
+			}
 		}
-		if(!eskf_zero_velocity_update(
-			&runtime->filter, engine->config.zero_velocity_std_mps) ||
-		   !eskf_zero_angular_rate_update(
-			&runtime->filter, mean_gyro_rps, engine->config.zero_gyro_std_rps))
+		if(update_applied)
 		{
-			return false;
+			runtime->last_zero_update_ticks = measurement->timestamp_ticks;
 		}
 		memset(runtime->zero_gyro_integral_rad, 0,
 		       sizeof(runtime->zero_gyro_integral_rad));
 		runtime->zero_gyro_duration_s = 0.0;
-		runtime->last_zero_update_ticks = measurement->timestamp_ticks;
-		if(count_zero_updates)
-		{
-			engine->diagnostics.zero_velocity_update_count++;
-			engine->diagnostics.zero_gyro_update_count++;
-		}
 	}
 	return true;
 }
@@ -706,6 +749,15 @@ static bool position_recovery_ready(
 	const fusion_runtime_t* runtime,
 	const fusion_position_measurement_t* measurement)
 {
+	/* Position-only GNSS cannot distinguish a coherent receiver restart error
+	 * or spoofed track from a true position discontinuity while moving.  Keep
+	 * automatic re-anchoring as a stationary startup/recovery mechanism; a
+	 * mobile reset needs independent velocity/yaw aiding or an external command. */
+	if(!runtime_allows_position_recovery(engine, runtime))
+	{
+		position_recovery_clear(engine);
+		return false;
+	}
 	if(!engine->position_recovery_active)
 	{
 		position_recovery_begin(engine, measurement);