onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
7cc3eb2a540bdb9a465787afd076924f8b76a8e7
gps/soc/software/fusion/tests/platform_scenario_matrix_test.c
T

552 lines
20 KiB
C
Raw Blame History

#include "fusion/fusion_engine.h"
#include <errno.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define TIMEBASE_HZ 1000000u
#define DURATION_S 50.0
#define PI 3.14159265358979323846264338327950288
typedef enum {
SCENARIO_STATIONARY = 0,
SCENARIO_PEDESTRIAN,
SCENARIO_CAR,
SCENARIO_AIRCRAFT,
SCENARIO_COUNT
} scenario_kind_t;
typedef struct {
double position_m[3];
double velocity_mps[3];
double yaw_rad;
} truth_t;
typedef struct {
double forward_accel_mps2;
double vertical_accel_mps2;
double yaw_rate_rps;
} command_t;
typedef struct {
uint32_t state;
} rng_t;
typedef struct {
double horizontal_rmse_m;
double final_horizontal_error_m;
double maximum_horizontal_error_m;
double maximum_burst_state_step_m;
uint64_t gps_accepted;
uint64_t gps_rejected;
uint64_t position_recoveries;
uint64_t zero_updates;
uint64_t zero_updates_after_startup;
bool finite;
bool recovered_after_burst;
} metrics_t;
static const char* scenario_name(scenario_kind_t scenario)
{
static const char* names[] = {"stationary", "pedestrian", "car", "aircraft"};
return names[(unsigned int)scenario];
}
static bool parse_scenario(const char* text, scenario_kind_t* scenario_out)
{
for(unsigned int index = 0u; index < SCENARIO_COUNT; index++)
{
if(strcmp(text, scenario_name((scenario_kind_t)index)) == 0)
{
*scenario_out = (scenario_kind_t)index;
return true;
}
}
return false;
}
static double square(double value)
{
return value * value;
}
static double rng_uniform(rng_t* rng)
{
rng->state = 1664525u * rng->state + 1013904223u;
return ((double)(rng->state >> 8u) + 0.5) / 16777216.0;
}
static double rng_normal(rng_t* rng)
{
double u1 = fmax(rng_uniform(rng), 1.0e-12);
double u2 = rng_uniform(rng);
return sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2);
}
static command_t command_at(scenario_kind_t scenario, double time_s)
{
command_t command = {0.0, 0.0, 0.0};
if(scenario == SCENARIO_PEDESTRIAN)
{
if((time_s >= 4.0) && (time_s < 6.0)) command.forward_accel_mps2 = 0.75;
else if((time_s >= 14.0) && (time_s < 16.0)) command.forward_accel_mps2 = -0.75;
else if((time_s >= 21.0) && (time_s < 23.0)) command.forward_accel_mps2 = 0.60;
else if((time_s >= 31.0) && (time_s < 33.0)) command.forward_accel_mps2 = -0.60;
if((time_s >= 24.0) && (time_s < 29.0)) command.yaw_rate_rps = 0.10;
}
else if(scenario == SCENARIO_CAR)
{
if((time_s >= 4.0) && (time_s < 9.0)) command.forward_accel_mps2 = 4.0;
else if((time_s >= 15.0) && (time_s < 18.0)) command.forward_accel_mps2 = -20.0 / 3.0;
else if((time_s >= 25.0) && (time_s < 29.0)) command.forward_accel_mps2 = 5.0;
else if((time_s >= 39.0) && (time_s < 42.0)) command.forward_accel_mps2 = -20.0 / 3.0;
if((time_s >= 31.0) && (time_s < 36.0)) command.yaw_rate_rps = 0.12;
}
else if(scenario == SCENARIO_AIRCRAFT)
{
if((time_s >= 4.0) && (time_s < 14.0)) command.forward_accel_mps2 = 8.0;
else if((time_s >= 26.0) && (time_s < 29.0)) command.forward_accel_mps2 = 4.0;
else if((time_s >= 42.0) && (time_s < 47.0)) command.forward_accel_mps2 = -6.0;
if((time_s >= 6.0) && (time_s < 10.0)) command.vertical_accel_mps2 = 2.0;
else if((time_s >= 18.0) && (time_s < 22.0)) command.vertical_accel_mps2 = -2.0;
if((time_s >= 20.0) && (time_s < 25.0)) command.yaw_rate_rps = 0.20;
else if((time_s >= 34.0) && (time_s < 36.0)) command.yaw_rate_rps = -0.35;
}
return command;
}
static void attitude_at(scenario_kind_t scenario, double time_s,
const truth_t* truth, double euler_rad[3],
double body_rate_rps[3])
{
memset(euler_rad, 0, 3u * sizeof(double));
memset(body_rate_rps, 0, 3u * sizeof(double));
euler_rad[2] = truth->yaw_rad;
command_t command = command_at(scenario, time_s);
body_rate_rps[2] = command.yaw_rate_rps;
if(scenario != SCENARIO_STATIONARY)
{
return;
}
const double roll_max = 20.0 * PI / 180.0;
const double pitch_max = 12.0 * PI / 180.0;
if((time_s >= 10.0) && (time_s < 11.0))
{
euler_rad[0] = roll_max * (time_s - 10.0);
body_rate_rps[0] = roll_max;
}
else if((time_s >= 11.0) && (time_s < 13.0)) euler_rad[0] = roll_max;
else if((time_s >= 13.0) && (time_s < 14.0))
{
euler_rad[0] = roll_max * (14.0 - time_s);
body_rate_rps[0] = -roll_max;
}
if((time_s >= 18.0) && (time_s < 19.0))
{
euler_rad[1] = pitch_max * (time_s - 18.0);
body_rate_rps[1] = pitch_max;
}
else if((time_s >= 19.0) && (time_s < 21.0)) euler_rad[1] = pitch_max;
else if((time_s >= 21.0) && (time_s < 22.0))
{
euler_rad[1] = pitch_max * (22.0 - time_s);
body_rate_rps[1] = -pitch_max;
}
}
static void rotation_body_to_nav(const double euler[3], double rotation[3][3])
{
double cr = cos(euler[0]);
double sr = sin(euler[0]);
double cp = cos(euler[1]);
double sp = sin(euler[1]);
double cy = cos(euler[2]);
double sy = sin(euler[2]);
rotation[0][0] = cy * cp;
rotation[0][1] = cy * sp * sr - sy * cr;
rotation[0][2] = cy * sp * cr + sy * sr;
rotation[1][0] = sy * cp;
rotation[1][1] = sy * sp * sr + cy * cr;
rotation[1][2] = sy * sp * cr - cy * sr;
rotation[2][0] = -sp;
rotation[2][1] = cp * sr;
rotation[2][2] = cp * cr;
}
static void propagate_truth(truth_t* truth, scenario_kind_t scenario,
double start_s, double dt_s)
{
command_t command = command_at(scenario, start_s + 0.5 * dt_s);
double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
double yaw_mid = truth->yaw_rad + 0.5 * command.yaw_rate_rps * dt_s;
double acceleration[3] = {
command.forward_accel_mps2 * cos(yaw_mid) - speed * command.yaw_rate_rps * sin(yaw_mid),
command.forward_accel_mps2 * sin(yaw_mid) + speed * command.yaw_rate_rps * cos(yaw_mid),
command.vertical_accel_mps2
};
for(unsigned int axis = 0u; axis < 3u; axis++)
{
truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
0.5 * acceleration[axis] * dt_s * dt_s;
truth->velocity_mps[axis] += acceleration[axis] * dt_s;
}
truth->yaw_rad += command.yaw_rate_rps * dt_s;
}
static bool externally_at_rest(scenario_kind_t scenario, double time_s)
{
if(time_s < 3.5) return true;
if(scenario != SCENARIO_STATIONARY) return false;
/* An external landed/rest supervisor owns this decision. Attitude changes
* and the vibration interval revoke the constraint; quiet IMU alone never
* invents rest for the moving scenarios. */
return !(((time_s >= 9.8) && (time_s < 15.0)) ||
((time_s >= 17.8) && (time_s < 23.0)) ||
((time_s >= 27.0) && (time_s < 30.0)));
}
static fusion_imu_measurement_t make_imu(scenario_kind_t scenario,
const truth_t* truth, double time_s,
uint64_t ticks, double gravity,
unsigned int sequence, rng_t* rng)
{
command_t command = command_at(scenario, time_s);
double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
double acceleration_nav[3] = {
command.forward_accel_mps2 * cos(truth->yaw_rad) -
speed * command.yaw_rate_rps * sin(truth->yaw_rad),
command.forward_accel_mps2 * sin(truth->yaw_rad) +
speed * command.yaw_rate_rps * cos(truth->yaw_rad),
command.vertical_accel_mps2 + gravity
};
double euler[3];
double body_rate[3];
double rotation[3][3];
attitude_at(scenario, time_s, truth, euler, body_rate);
rotation_body_to_nav(euler, rotation);
fusion_imu_measurement_t imu;
memset(&imu, 0, sizeof(imu));
imu.timestamp_ticks = ticks;
for(unsigned int body = 0u; body < 3u; body++)
{
for(unsigned int nav = 0u; nav < 3u; nav++)
{
imu.accel_mps2[body] += rotation[nav][body] * acceleration_nav[nav];
}
static const double accel_bias[3] = {0.035, -0.025, 0.055};
static const double gyro_bias[3] = {0.0008, -0.0006, 0.0007};
imu.accel_mps2[body] += accel_bias[body] + 0.012 * rng_normal(rng);
imu.gyro_rps[body] = body_rate[body] + gyro_bias[body] +
0.00035 * rng_normal(rng);
}
if((scenario == SCENARIO_STATIONARY) &&
(time_s >= 27.0) && (time_s < 29.0))
{
imu.accel_mps2[0] += 0.80 * sin(2.0 * PI * 7.0 * time_s);
imu.accel_mps2[1] += 0.55 * sin(2.0 * PI * 11.0 * time_s);
imu.gyro_rps[2] += 0.035 * sin(2.0 * PI * 5.0 * time_s);
}
imu.temperature_c = 25.0 + 0.5 * sin(0.08 * time_s);
imu.sequence = sequence;
imu.vehicle_at_rest = externally_at_rest(scenario, time_s);
return imu;
}
static fusion_position_measurement_t make_gps(const truth_t* truth,
double time_s, uint64_t ticks,
rng_t* rng, double gps_position[3])
{
double horizontal_std = time_s < 8.0 ? 8.0 : 4.0;
double vertical_std = time_s < 8.0 ? 12.0 : 7.0;
for(unsigned int axis = 0u; axis < 3u; axis++)
{
double std = axis < 2u ? horizontal_std : vertical_std;
gps_position[axis] = truth->position_m[axis] + std * rng_normal(rng);
}
if((time_s >= 32.0) && (time_s < 34.0))
{
gps_position[0] += 130.0;
gps_position[1] -= 90.0;
gps_position[2] += 55.0;
}
fusion_position_measurement_t gps;
memset(&gps, 0, sizeof(gps));
gps.timestamp_ticks = ticks;
memcpy(gps.position_enu_m, gps_position, sizeof(gps.position_enu_m));
gps.covariance_enu_m2[0] = square(horizontal_std);
gps.covariance_enu_m2[4] = square(horizontal_std);
gps.covariance_enu_m2[8] = square(vertical_std);
/* Exactly four satellites have zero residual degrees of freedom. This is
* therefore a valid WLS output with covariance but no redundant FDE test. */
gps.source_quality = 100.0;
gps.source_valid = true;
return gps;
}
static void local_to_geodetic(const double position[3], double* latitude_deg,
double* longitude_deg, double* height_m)
{
const double origin_latitude_deg = 40.78925034214207;
const double origin_longitude_deg = 29.46277530841728;
const double origin_height_m = 100.0;
const double latitude_rad = origin_latitude_deg * PI / 180.0;
*latitude_deg = origin_latitude_deg + position[1] / 6378137.0 * 180.0 / PI;
*longitude_deg = origin_longitude_deg +
position[0] / (6378137.0 * cos(latitude_rad)) * 180.0 / PI;
*height_m = origin_height_m + position[2];
}
static void write_location(FILE* log_file, const char* source,
scenario_kind_t scenario, double time_s,
const double position[3], unsigned int gps_ms,
unsigned int imu_ms)
{
if(log_file == NULL) return;
double latitude;
double longitude;
double height;
local_to_geodetic(position, &latitude, &longitude, &height);
fprintf(log_file,
"[LOCATION] source=%s scenario=%s t=%.3f lat=%.12f lon=%.12f "
"height=%.3f gdop=3.2 pdop=2.6 hdop=2.0 vdop=1.7 tdop=1.8 "
"tracked=4 used=4 gps_ms=%u imu_ms=%u\n",
source, scenario_name(scenario), time_s, latitude, longitude,
height, gps_ms, imu_ms);
}
static metrics_t run_case(scenario_kind_t scenario, unsigned int gps_ms,
unsigned int imu_ms, FILE* log_file)
{
static fusion_engine_t engine;
memset(&engine, 0, sizeof(engine));
fusion_engine_config_t config;
fusion_engine_default_config(&config, TIMEBASE_HZ);
config.calibration_duration_s = 2.0;
/* Calibration is time-based for asynchronous sensors; retain enough samples
* to estimate variance without silently assuming a particular ODR. */
config.calibration_min_samples = 2u * 1000u / imu_ms + 1u;
metrics_t metrics;
memset(&metrics, 0, sizeof(metrics));
if(!fusion_engine_init(&engine, &config)) return metrics;
truth_t truth;
memset(&truth, 0, sizeof(truth));
rng_t imu_rng = {0x7a31f20du ^ ((uint32_t)scenario << 24u) ^ imu_ms};
rng_t gps_rng = {0xb41d93c7u ^ ((uint32_t)scenario << 20u) ^ gps_ms};
uint64_t imu_period_ticks = (uint64_t)imu_ms * 1000u;
uint64_t gps_period_ticks = (uint64_t)gps_ms * 1000u;
uint64_t next_imu_ticks = 0u;
uint64_t next_gps_ticks = 0u;
uint64_t next_plot_ticks = 0u;
uint64_t end_ticks = (uint64_t)(DURATION_S * TIMEBASE_HZ);
uint64_t last_ticks = 0u;
unsigned int imu_sequence = 0u;
double error_squared_sum = 0.0;
uint64_t error_count = 0u;
uint64_t accepted_at_burst_end = 0u;
uint64_t zero_updates_at_startup_end = 0u;
double previous_horizontal[2] = {0.0, 0.0};
bool have_previous = false;
if(log_file != NULL)
{
fprintf(log_file, "# ESKF synthetic replay; blue=GPS red=IMU-corrected green=truth\n");
fprintf(log_file, "# scenario=%s gps_ms=%u imu_ms=%u satellites=4\n",
scenario_name(scenario), gps_ms, imu_ms);
}
while(true)
{
uint64_t ticks = next_imu_ticks;
if(next_gps_ticks < ticks) ticks = next_gps_ticks;
if(next_plot_ticks < ticks) ticks = next_plot_ticks;
if(ticks > end_ticks) break;
double start_s = (double)last_ticks / TIMEBASE_HZ;
double dt_s = (double)(ticks - last_ticks) / TIMEBASE_HZ;
propagate_truth(&truth, scenario, start_s, dt_s);
last_ticks = ticks;
double time_s = (double)ticks / TIMEBASE_HZ;
if(ticks == next_gps_ticks)
{
/* A 3 s dropout exercises asynchronous dead reckoning. */
if(!((time_s >= 25.0) && (time_s < 28.0)))
{
double gps_position[3];
fusion_position_measurement_t gps = make_gps(
&truth, time_s, ticks, &gps_rng, gps_position);
eskf_update_result_t result = ESKF_UPDATE_INVALID;
(void)fusion_engine_push_position(&engine, &gps, &result, NULL);
write_location(log_file, "gps", scenario, time_s, gps_position,
gps_ms, imu_ms);
}
next_gps_ticks += gps_period_ticks;
}
if(ticks == next_imu_ticks)
{
fusion_imu_measurement_t imu = make_imu(
scenario, &truth, time_s, ticks, config.eskf.gravity_mps2,
imu_sequence++, &imu_rng);
if(!fusion_engine_push_imu(&engine, &imu)) break;
next_imu_ticks += imu_period_ticks;
}
if(engine.state == FUSION_ENGINE_RUNNING)
{
if((time_s >= 4.0) && (zero_updates_at_startup_end == 0u))
zero_updates_at_startup_end =
engine.diagnostics.zero_velocity_update_count;
double horizontal_error = hypot(
engine.current.filter.position_m[0] - truth.position_m[0],
engine.current.filter.position_m[1] - truth.position_m[1]);
metrics.maximum_horizontal_error_m = fmax(
metrics.maximum_horizontal_error_m, horizontal_error);
if(time_s >= 8.0)
{
error_squared_sum += square(horizontal_error);
error_count++;
}
if(have_previous && (time_s >= 32.0) && (time_s < 34.2))
{
double step = hypot(
engine.current.filter.position_m[0] - previous_horizontal[0],
engine.current.filter.position_m[1] - previous_horizontal[1]);
metrics.maximum_burst_state_step_m = fmax(
metrics.maximum_burst_state_step_m, step);
}
previous_horizontal[0] = engine.current.filter.position_m[0];
previous_horizontal[1] = engine.current.filter.position_m[1];
have_previous = true;
if((time_s >= 34.0) && (accepted_at_burst_end == 0u))
accepted_at_burst_end = engine.diagnostics.position_accepted;
}
if(ticks == next_plot_ticks)
{
write_location(log_file, "trajectory", scenario, time_s,
truth.position_m, gps_ms, imu_ms);
if(engine.state == FUSION_ENGINE_RUNNING)
write_location(log_file, "imu_corrected", scenario, time_s,
engine.current.filter.position_m, gps_ms, imu_ms);
next_plot_ticks += 200000u;
}
}
double final_error = hypot(
engine.current.filter.position_m[0] - truth.position_m[0],
engine.current.filter.position_m[1] - truth.position_m[1]);
metrics.horizontal_rmse_m = error_count > 0u
? sqrt(error_squared_sum / (double)error_count) : INFINITY;
metrics.final_horizontal_error_m = final_error;
metrics.gps_accepted = engine.diagnostics.position_accepted;
metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
metrics.position_recoveries = engine.diagnostics.position_recovery_count;
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
metrics.zero_updates_after_startup = metrics.zero_updates >= zero_updates_at_startup_end
? metrics.zero_updates - zero_updates_at_startup_end : metrics.zero_updates;
metrics.recovered_after_burst = engine.diagnostics.position_accepted >
accepted_at_burst_end;
metrics.finite = (engine.state == FUSION_ENGINE_RUNNING) &&
eskf_state_is_finite(&engine.current.filter) && isfinite(final_error);
return metrics;
}
static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms,
unsigned int imu_ms, const metrics_t* metrics)
{
printf("%-10s GPS=%4ums IMU=%3ums rmse=%7.2fm final=%7.2fm max=%7.2fm "
"burst-step=%6.2fm gps=%3llu/%2llu recovery=%llu zupt=%llu(+%llu) finite=%u\n",
scenario_name(scenario), gps_ms, imu_ms, metrics->horizontal_rmse_m,
metrics->final_horizontal_error_m, metrics->maximum_horizontal_error_m,
metrics->maximum_burst_state_step_m,
(unsigned long long)metrics->gps_accepted,
(unsigned long long)metrics->gps_rejected,
(unsigned long long)metrics->position_recoveries,
(unsigned long long)metrics->zero_updates,
(unsigned long long)metrics->zero_updates_after_startup,
metrics->finite ? 1u : 0u);
}
static int run_matrix(void)
{
static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u};
static const unsigned int imu_periods_ms[] = {19u, 20u, 200u, 300u};
bool passed = true;
printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n");
for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++)
{
for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++)
{
for(unsigned int imu_index = 0u; imu_index < 4u; imu_index++)
{
metrics_t metrics = run_case((scenario_kind_t)scenario,
gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL);
print_metrics((scenario_kind_t)scenario, gps_periods_ms[gps_index],
imu_periods_ms[imu_index], &metrics);
bool common_ok = metrics.finite && metrics.recovered_after_burst &&
(metrics.position_recoveries == 0u) &&
(metrics.maximum_burst_state_step_m < 40.0);
bool rest_ok = (scenario == SCENARIO_STATIONARY)
? (metrics.zero_updates_after_startup > 0u)
: (metrics.zero_updates_after_startup == 0u);
bool production_rate_ok = (imu_periods_ms[imu_index] > 20u) ||
(metrics.final_horizontal_error_m < 25.0);
passed = passed && common_ok && rest_ok && production_rate_ok;
}
}
}
if(!passed)
{
fprintf(stderr, "FAIL: platform scenario matrix contract.\n");
return 1;
}
printf("PASS: 19/20 ms production rates stayed bounded; 200/300 ms coarse IMU "
"rates remained finite and their dynamic accuracy loss is reported, not hidden.\n");
return 0;
}
int main(int argc, char** argv)
{
if(argc == 1) return run_matrix();
if((argc == 6) && (strcmp(argv[1], "--log") == 0))
{
scenario_kind_t scenario;
if(!parse_scenario(argv[2], &scenario))
{
fprintf(stderr, "unknown scenario: %s\n", argv[2]);
return 2;
}
unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
!((imu_ms == 19u) || (imu_ms == 20u) ||
(imu_ms == 200u) || (imu_ms == 300u)))
{
fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/200/300 ms\n");
return 2;
}
FILE* log_file = fopen(argv[5], "w");
if(log_file == NULL)
{
fprintf(stderr, "cannot create %s: %s\n", argv[5], strerror(errno));
return 2;
}
metrics_t metrics = run_case(scenario, gps_ms, imu_ms, log_file);
if(fclose(log_file) != 0)
{
fprintf(stderr, "cannot close %s\n", argv[5]);
return 2;
}
print_metrics(scenario, gps_ms, imu_ms, &metrics);
printf("wrote replay log: %s\n", argv[5]);
return metrics.finite ? 0 : 1;
}
fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft "
"200|500|1000 19|20|200|300 output.log]\n", argv[0]);
return 2;
}
Reference in New Issue View Git Blame Copy Permalink