437 lines
13 KiB
C
437 lines
13 KiB
C
#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;
|
|
}
|