653 lines
23 KiB
C
653 lines
23 KiB
C
#include <math.h>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include "fusion/fusion_engine.h"
|
|
|
|
#define SYNTH_PI 3.14159265358979323846264338327950288
|
|
#define SYNTH_TIMEBASE_HZ 1000000u
|
|
#define SYNTH_IMU_HZ 200u
|
|
#define SYNTH_DURATION_S 60u
|
|
#define SYNTH_GPS_EVENT_CAPACITY 96u
|
|
|
|
typedef struct {
|
|
uint32_t state;
|
|
} synth_rng_t;
|
|
|
|
typedef struct {
|
|
double position_m[3];
|
|
double velocity_mps[3];
|
|
double yaw_rad;
|
|
} synth_truth_t;
|
|
|
|
typedef struct {
|
|
fusion_position_measurement_t measurement;
|
|
uint64_t delivery_ticks;
|
|
bool injected_outlier;
|
|
} synth_gps_event_t;
|
|
|
|
typedef struct {
|
|
double horizontal_rmse_m;
|
|
double vertical_rmse_m;
|
|
double final_horizontal_error_m;
|
|
double final_vertical_error_m;
|
|
double final_speed_mps;
|
|
double max_rejected_state_jump_m;
|
|
double covariance_symmetry_error;
|
|
uint64_t accepted_gps;
|
|
uint64_t rejected_gps;
|
|
uint64_t replay_count;
|
|
uint64_t replayed_imu_count;
|
|
uint64_t zero_updates;
|
|
unsigned int injected_outliers;
|
|
unsigned int rejected_injected_outliers;
|
|
unsigned int false_stationary_samples;
|
|
double moving_detection_time_s;
|
|
double stationary_reentry_time_s;
|
|
bool covariance_positive_semidefinite;
|
|
bool state_finite;
|
|
} synth_metrics_t;
|
|
|
|
typedef struct {
|
|
double final_horizontal_error_m;
|
|
double final_vertical_error_m;
|
|
double final_speed_mps;
|
|
uint64_t accepted_gps;
|
|
uint64_t zero_updates;
|
|
unsigned int nonstationary_samples;
|
|
bool covariance_positive_semidefinite;
|
|
bool state_finite;
|
|
} stationary_metrics_t;
|
|
|
|
static int failures;
|
|
|
|
#define CHECK(condition, message) do { \
|
|
if(!(condition)) { \
|
|
fprintf(stderr, "FAIL: %s\n", message); \
|
|
failures++; \
|
|
} \
|
|
} while(0)
|
|
|
|
static double square(double value)
|
|
{
|
|
return value * value;
|
|
}
|
|
|
|
static double norm2(double first, double second)
|
|
{
|
|
return sqrt(first * first + second * second);
|
|
}
|
|
|
|
static double norm3(const double value[3])
|
|
{
|
|
return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
|
|
}
|
|
|
|
static uint32_t rng_next(synth_rng_t* rng)
|
|
{
|
|
uint32_t value = rng->state;
|
|
value ^= value << 13;
|
|
value ^= value >> 17;
|
|
value ^= value << 5;
|
|
rng->state = value;
|
|
return value;
|
|
}
|
|
|
|
static double rng_uniform(synth_rng_t* rng)
|
|
{
|
|
return ((double)rng_next(rng) + 0.5) / 4294967296.0;
|
|
}
|
|
|
|
static double rng_gaussian(synth_rng_t* rng)
|
|
{
|
|
double first = fmax(rng_uniform(rng), 1.0e-12);
|
|
double second = rng_uniform(rng);
|
|
return sqrt(-2.0 * log(first)) * cos(2.0 * SYNTH_PI * second);
|
|
}
|
|
|
|
static void trajectory_input(double time_s,
|
|
double acceleration_nav_mps2[3],
|
|
double* yaw_rate_rps)
|
|
{
|
|
memset(acceleration_nav_mps2, 0, 3u * sizeof(double));
|
|
*yaw_rate_rps = 0.0;
|
|
if((time_s >= 5.0) && (time_s < 7.0))
|
|
{
|
|
acceleration_nav_mps2[0] = 1.0;
|
|
}
|
|
else if((time_s >= 10.0) && (time_s < 12.0))
|
|
{
|
|
acceleration_nav_mps2[0] = -1.0;
|
|
}
|
|
else if((time_s >= 15.0) && (time_s < 17.0))
|
|
{
|
|
*yaw_rate_rps = 0.25 * SYNTH_PI;
|
|
}
|
|
else if((time_s >= 17.0) && (time_s < 19.0))
|
|
{
|
|
acceleration_nav_mps2[1] = 0.8;
|
|
}
|
|
else if((time_s >= 22.0) && (time_s < 24.0))
|
|
{
|
|
acceleration_nav_mps2[1] = -0.8;
|
|
}
|
|
}
|
|
|
|
static void truth_step(synth_truth_t* truth,
|
|
double dt_s,
|
|
const double acceleration_nav_mps2[3],
|
|
double yaw_rate_rps)
|
|
{
|
|
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
|
{
|
|
truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
|
|
0.5 * acceleration_nav_mps2[axis] * dt_s * dt_s;
|
|
truth->velocity_mps[axis] += acceleration_nav_mps2[axis] * dt_s;
|
|
}
|
|
truth->yaw_rad += yaw_rate_rps * dt_s;
|
|
}
|
|
|
|
static fusion_position_measurement_t make_position(
|
|
uint64_t timestamp_ticks,
|
|
const synth_truth_t* truth,
|
|
synth_rng_t* gps_rng,
|
|
bool outlier)
|
|
{
|
|
fusion_position_measurement_t position;
|
|
memset(&position, 0, sizeof(position));
|
|
position.timestamp_ticks = timestamp_ticks;
|
|
position.position_enu_m[0] = truth->position_m[0] + 1.2 * rng_gaussian(gps_rng);
|
|
position.position_enu_m[1] = truth->position_m[1] + 1.2 * rng_gaussian(gps_rng);
|
|
position.position_enu_m[2] = truth->position_m[2] + 2.0 * rng_gaussian(gps_rng);
|
|
if(outlier)
|
|
{
|
|
position.position_enu_m[0] += 150.0;
|
|
position.position_enu_m[1] -= 120.0;
|
|
position.position_enu_m[2] += 80.0;
|
|
}
|
|
position.covariance_enu_m2[0] = square(1.2);
|
|
position.covariance_enu_m2[4] = square(1.2);
|
|
position.covariance_enu_m2[8] = square(2.0);
|
|
position.source_quality = outlier ? 0.7 : 1.0;
|
|
position.source_valid = true;
|
|
return position;
|
|
}
|
|
|
|
static bool covariance_is_psd(const eskf_t* filter,
|
|
double* symmetry_error_out)
|
|
{
|
|
double lower[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
|
double max_symmetry_error = 0.0;
|
|
for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
|
|
{
|
|
for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
|
|
{
|
|
double error = fabs(filter->covariance[row][col] -
|
|
filter->covariance[col][row]);
|
|
if(error > max_symmetry_error)
|
|
{
|
|
max_symmetry_error = error;
|
|
}
|
|
}
|
|
}
|
|
if(symmetry_error_out != NULL)
|
|
{
|
|
*symmetry_error_out = max_symmetry_error;
|
|
}
|
|
|
|
/* A tiny diagonal jitter makes the semidefinite check numerically robust. */
|
|
for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
|
|
{
|
|
for(unsigned int col = 0u ; col <= row ; col++)
|
|
{
|
|
double value = filter->covariance[row][col];
|
|
if(row == col)
|
|
{
|
|
value += 1.0e-10;
|
|
}
|
|
for(unsigned int k = 0u ; k < col ; k++)
|
|
{
|
|
value -= lower[row][k] * lower[col][k];
|
|
}
|
|
if(row == col)
|
|
{
|
|
if(!isfinite(value) || (value <= 0.0))
|
|
{
|
|
return false;
|
|
}
|
|
lower[row][col] = sqrt(value);
|
|
}
|
|
else
|
|
{
|
|
lower[row][col] = value / lower[col][col];
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static void initialize_synthetic_engine(fusion_engine_t* engine)
|
|
{
|
|
fusion_engine_config_t config;
|
|
fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
|
|
config.calibration_duration_s = 2.0;
|
|
config.calibration_min_samples = SYNTH_IMU_HZ;
|
|
CHECK(fusion_engine_init(engine, &config), "synthetic fusion engine initializes");
|
|
|
|
fusion_position_measurement_t origin;
|
|
memset(&origin, 0, sizeof(origin));
|
|
origin.timestamp_ticks = 0u;
|
|
origin.covariance_enu_m2[0] = square(1.2);
|
|
origin.covariance_enu_m2[4] = square(1.2);
|
|
origin.covariance_enu_m2[8] = square(2.0);
|
|
origin.source_quality = 1.0;
|
|
origin.source_valid = true;
|
|
CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL),
|
|
"initial GPS is retained during calibration");
|
|
}
|
|
|
|
static synth_metrics_t run_dynamic_scenario(uint32_t scenario_seed)
|
|
{
|
|
static fusion_engine_t engine;
|
|
initialize_synthetic_engine(&engine);
|
|
synth_truth_t truth = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0};
|
|
synth_rng_t imu_rng = {0x13579bdfu ^ (0x9e3779b9u * scenario_seed)};
|
|
synth_rng_t gps_rng = {0x2468ace1u ^ (0x85ebca6bu * scenario_seed)};
|
|
synth_rng_t timing_rng = {0xa5a5c3c3u ^ (0xc2b2ae35u * scenario_seed)};
|
|
const double accel_bias[3] = {0.030, -0.025, 0.060};
|
|
const double gyro_bias[3] = {0.0010, -0.0007, 0.0009};
|
|
const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / SYNTH_IMU_HZ;
|
|
const double dt_s = 1.0 / (double)SYNTH_IMU_HZ;
|
|
const uint64_t final_tick = (uint64_t)SYNTH_DURATION_S * SYNTH_TIMEBASE_HZ;
|
|
uint64_t next_gps_tick = 3000000u;
|
|
unsigned int gps_sequence = 0u;
|
|
synth_gps_event_t events[SYNTH_GPS_EVENT_CAPACITY];
|
|
unsigned int event_head = 0u;
|
|
unsigned int event_count = 0u;
|
|
double horizontal_error_sum = 0.0;
|
|
double vertical_error_sum = 0.0;
|
|
uint64_t error_sample_count = 0u;
|
|
synth_metrics_t metrics;
|
|
memset(&metrics, 0, sizeof(metrics));
|
|
metrics.moving_detection_time_s = NAN;
|
|
metrics.stationary_reentry_time_s = NAN;
|
|
#ifdef SYNTH_TRACE
|
|
fusion_motion_state_t traced_state = FUSION_MOTION_BOOTSTRAP;
|
|
#endif
|
|
|
|
for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
|
|
{
|
|
double acceleration_nav[3] = {0.0, 0.0, 0.0};
|
|
double yaw_rate = 0.0;
|
|
if(tick != 0u)
|
|
{
|
|
double midpoint_s = ((double)tick - 0.5 * (double)imu_period_ticks) /
|
|
(double)SYNTH_TIMEBASE_HZ;
|
|
trajectory_input(midpoint_s, acceleration_nav, &yaw_rate);
|
|
truth_step(&truth, dt_s, acceleration_nav, yaw_rate);
|
|
}
|
|
|
|
double midpoint_yaw = truth.yaw_rad - 0.5 * yaw_rate * dt_s;
|
|
double cosine = cos(midpoint_yaw);
|
|
double sine = sin(midpoint_yaw);
|
|
double specific_force_nav[3] = {
|
|
acceleration_nav[0], acceleration_nav[1], engine.config.eskf.gravity_mps2
|
|
};
|
|
fusion_imu_measurement_t imu;
|
|
memset(&imu, 0, sizeof(imu));
|
|
imu.timestamp_ticks = tick;
|
|
imu.accel_mps2[0] = cosine * specific_force_nav[0] +
|
|
sine * specific_force_nav[1];
|
|
imu.accel_mps2[1] = -sine * specific_force_nav[0] +
|
|
cosine * specific_force_nav[1];
|
|
imu.accel_mps2[2] = specific_force_nav[2];
|
|
imu.gyro_rps[2] = yaw_rate;
|
|
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
|
{
|
|
imu.accel_mps2[axis] += accel_bias[axis] + 0.025 * rng_gaussian(&imu_rng);
|
|
imu.gyro_rps[axis] += gyro_bias[axis] + 0.0008 * rng_gaussian(&imu_rng);
|
|
}
|
|
imu.temperature_c = 25.0 + 0.5 * sin((double)tick / 10000000.0);
|
|
imu.sequence = (uint32_t)(tick / imu_period_ticks);
|
|
imu.vehicle_at_rest =
|
|
(norm3(truth.velocity_mps) < 0.05) &&
|
|
(norm3(acceleration_nav) < 0.05) && (fabs(yaw_rate) < 0.02);
|
|
CHECK(fusion_engine_push_imu(&engine, &imu), "dynamic synthetic IMU accepted");
|
|
if(engine.state == FUSION_ENGINE_FAULT)
|
|
{
|
|
break;
|
|
}
|
|
|
|
if((tick >= next_gps_tick) &&
|
|
((event_head + event_count) < SYNTH_GPS_EVENT_CAPACITY))
|
|
{
|
|
bool outlier = (gps_sequence == 8u) || (gps_sequence == 27u);
|
|
unsigned int index = event_head + event_count;
|
|
events[index].measurement = make_position(tick, &truth, &gps_rng, outlier);
|
|
double delay_s = 0.18 + 0.17 * rng_uniform(&timing_rng);
|
|
events[index].delivery_ticks = tick +
|
|
(uint64_t)(delay_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
|
|
events[index].injected_outlier = outlier;
|
|
event_count++;
|
|
if(outlier)
|
|
{
|
|
metrics.injected_outliers++;
|
|
}
|
|
gps_sequence++;
|
|
double next_period_s = 0.80 + 0.40 * rng_uniform(&timing_rng);
|
|
next_gps_tick = tick +
|
|
(uint64_t)(next_period_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
|
|
}
|
|
|
|
while((event_count != 0u) && (events[event_head].delivery_ticks <= tick))
|
|
{
|
|
double before[3];
|
|
memcpy(before, engine.current.filter.position_m, sizeof(before));
|
|
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
|
double nis = NAN;
|
|
CHECK(fusion_engine_push_position(&engine, &events[event_head].measurement,
|
|
&result, &nis),
|
|
"delayed asynchronous GPS is processed");
|
|
if(events[event_head].injected_outlier &&
|
|
(result == ESKF_UPDATE_REJECTED_NIS))
|
|
{
|
|
metrics.rejected_injected_outliers++;
|
|
double state_jump = norm2(
|
|
engine.current.filter.position_m[0] - before[0],
|
|
engine.current.filter.position_m[1] - before[1]);
|
|
if(state_jump > metrics.max_rejected_state_jump_m)
|
|
{
|
|
metrics.max_rejected_state_jump_m = state_jump;
|
|
}
|
|
}
|
|
event_head++;
|
|
event_count--;
|
|
}
|
|
|
|
if(engine.state == FUSION_ENGINE_RUNNING)
|
|
{
|
|
double time_s = (double)tick / (double)SYNTH_TIMEBASE_HZ;
|
|
fusion_motion_state_t state = engine.current.motion.state;
|
|
#ifdef SYNTH_TRACE
|
|
if(state != traced_state)
|
|
{
|
|
printf("TRACE t=%.3f state=%s speed=%.3f gnss=%u confirm=%u accel=%.3f jerk=%.3f\n",
|
|
time_s, fusion_motion_state_name(state),
|
|
eskf_speed_mps(&engine.current.filter),
|
|
(unsigned int)engine.current.gnss_evidence,
|
|
engine.current.gnss_moving_count,
|
|
engine.current.motion.last_linear_accel_norm_mps2,
|
|
engine.current.motion.last_jerk_norm_mps3);
|
|
traced_state = state;
|
|
}
|
|
#endif
|
|
if(isnan(metrics.moving_detection_time_s) && (time_s >= 5.0) &&
|
|
(state == FUSION_MOTION_MOVING))
|
|
{
|
|
metrics.moving_detection_time_s = time_s;
|
|
}
|
|
if((time_s >= 7.25) && (time_s <= 9.75) &&
|
|
(state == FUSION_MOTION_STATIONARY))
|
|
{
|
|
metrics.false_stationary_samples++;
|
|
}
|
|
if((time_s >= 19.25) && (time_s <= 21.75) &&
|
|
(state == FUSION_MOTION_STATIONARY))
|
|
{
|
|
metrics.false_stationary_samples++;
|
|
}
|
|
if(isnan(metrics.stationary_reentry_time_s) && (time_s >= 24.0) &&
|
|
(state == FUSION_MOTION_STATIONARY))
|
|
{
|
|
metrics.stationary_reentry_time_s = time_s;
|
|
}
|
|
if(time_s >= 3.0)
|
|
{
|
|
double east_error = engine.current.filter.position_m[0] - truth.position_m[0];
|
|
double north_error = engine.current.filter.position_m[1] - truth.position_m[1];
|
|
double up_error = engine.current.filter.position_m[2] - truth.position_m[2];
|
|
horizontal_error_sum += square(east_error) + square(north_error);
|
|
vertical_error_sum += square(up_error);
|
|
error_sample_count++;
|
|
}
|
|
}
|
|
}
|
|
|
|
metrics.accepted_gps = engine.diagnostics.position_accepted;
|
|
metrics.rejected_gps = engine.diagnostics.position_nis_rejected;
|
|
metrics.replay_count = engine.diagnostics.replay_count;
|
|
metrics.replayed_imu_count = engine.diagnostics.replayed_imu_count;
|
|
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
|
|
metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
|
|
metrics.covariance_positive_semidefinite = covariance_is_psd(
|
|
&engine.current.filter, &metrics.covariance_symmetry_error);
|
|
if(error_sample_count != 0u)
|
|
{
|
|
metrics.horizontal_rmse_m = sqrt(horizontal_error_sum / (double)error_sample_count);
|
|
metrics.vertical_rmse_m = sqrt(vertical_error_sum / (double)error_sample_count);
|
|
}
|
|
metrics.final_horizontal_error_m = norm2(
|
|
engine.current.filter.position_m[0] - truth.position_m[0],
|
|
engine.current.filter.position_m[1] - truth.position_m[1]);
|
|
metrics.final_vertical_error_m = fabs(
|
|
engine.current.filter.position_m[2] - truth.position_m[2]);
|
|
metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps);
|
|
return metrics;
|
|
}
|
|
|
|
static void check_dynamic_metrics(const synth_metrics_t* metrics)
|
|
{
|
|
CHECK(metrics->state_finite, "60 s synthetic state remains finite");
|
|
CHECK(metrics->covariance_positive_semidefinite,
|
|
"60 s covariance remains positive semidefinite");
|
|
CHECK(metrics->covariance_symmetry_error < 1.0e-11,
|
|
"60 s covariance remains symmetric");
|
|
CHECK(isfinite(metrics->moving_detection_time_s) &&
|
|
(metrics->moving_detection_time_s <= 5.010),
|
|
"sudden acceleration is detected within two 200 Hz samples");
|
|
CHECK(metrics->false_stationary_samples == 0u,
|
|
"constant-velocity legs never enable stationary updates");
|
|
CHECK(isfinite(metrics->stationary_reentry_time_s) &&
|
|
(metrics->stationary_reentry_time_s >= 25.0) &&
|
|
(metrics->stationary_reentry_time_s <= 28.0),
|
|
"final stop enters stationary after hysteresis dwell");
|
|
CHECK(metrics->injected_outliers == 2u,
|
|
"two synthetic multipath-like GPS outliers were injected");
|
|
CHECK(metrics->rejected_injected_outliers == metrics->injected_outliers,
|
|
"NIS rejects every injected GPS outlier");
|
|
CHECK(metrics->max_rejected_state_jump_m < 1.0e-8,
|
|
"rejected delayed GPS does not perturb the current state");
|
|
CHECK(metrics->accepted_gps >= 45u,
|
|
"most asynchronous GPS measurements are accepted");
|
|
CHECK(metrics->rejected_gps >= metrics->injected_outliers,
|
|
"GPS reject diagnostics include injected outliers");
|
|
CHECK(metrics->replay_count >= 50u && metrics->replayed_imu_count > 1000u,
|
|
"delayed GPS exercises fixed-lag replay extensively");
|
|
CHECK(metrics->zero_updates > 150u,
|
|
"stationary periods apply PX4-rate 5 Hz ZUPT updates");
|
|
CHECK(metrics->horizontal_rmse_m < 2.0,
|
|
"bias/noise/delay trajectory horizontal RMSE is bounded");
|
|
CHECK(metrics->vertical_rmse_m < 2.5,
|
|
"bias/noise/delay trajectory vertical RMSE is bounded");
|
|
CHECK(metrics->final_horizontal_error_m < 1.5,
|
|
"final stationary horizontal error is bounded");
|
|
CHECK(metrics->final_vertical_error_m < 2.0,
|
|
"final stationary vertical error is bounded");
|
|
CHECK(metrics->final_speed_mps < 0.05,
|
|
"final stationary ZUPT speed is near zero");
|
|
}
|
|
|
|
static stationary_metrics_t run_stationary_500hz_scenario(void)
|
|
{
|
|
static fusion_engine_t engine;
|
|
fusion_engine_config_t config;
|
|
fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
|
|
config.calibration_duration_s = 2.0;
|
|
config.calibration_min_samples = 500u;
|
|
CHECK(fusion_engine_init(&engine, &config), "500 Hz stationary engine initializes");
|
|
|
|
fusion_position_measurement_t origin;
|
|
memset(&origin, 0, sizeof(origin));
|
|
origin.covariance_enu_m2[0] = square(0.8);
|
|
origin.covariance_enu_m2[4] = square(0.8);
|
|
origin.covariance_enu_m2[8] = square(1.5);
|
|
origin.source_quality = 1.0;
|
|
origin.source_valid = true;
|
|
CHECK(fusion_engine_push_position(&engine, &origin, NULL, NULL),
|
|
"500 Hz stationary origin accepted");
|
|
|
|
const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / 500u;
|
|
const uint64_t final_tick = 30000000u;
|
|
uint64_t next_gps_tick = 3000000u;
|
|
uint64_t delivery_tick = 0u;
|
|
bool gps_pending = false;
|
|
fusion_position_measurement_t pending_gps;
|
|
synth_rng_t imu_rng = {0x10203040u};
|
|
synth_rng_t gps_rng = {0x55667788u};
|
|
const double accel_bias[3] = {0.020, -0.015, 0.050};
|
|
const double gyro_bias[3] = {0.0008, -0.0005, 0.0006};
|
|
stationary_metrics_t metrics;
|
|
memset(&metrics, 0, sizeof(metrics));
|
|
|
|
for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
|
|
{
|
|
fusion_imu_measurement_t imu;
|
|
memset(&imu, 0, sizeof(imu));
|
|
imu.timestamp_ticks = tick;
|
|
imu.accel_mps2[2] = config.eskf.gravity_mps2;
|
|
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
|
{
|
|
imu.accel_mps2[axis] += accel_bias[axis] + 0.020 * rng_gaussian(&imu_rng);
|
|
imu.gyro_rps[axis] = gyro_bias[axis] + 0.0006 * rng_gaussian(&imu_rng);
|
|
}
|
|
imu.temperature_c = 24.0;
|
|
imu.sequence = (uint32_t)(tick / imu_period_ticks);
|
|
imu.vehicle_at_rest = true;
|
|
CHECK(fusion_engine_push_imu(&engine, &imu), "500 Hz stationary IMU accepted");
|
|
|
|
if((tick >= next_gps_tick) && !gps_pending)
|
|
{
|
|
synth_truth_t stationary_truth = {{0.0, 0.0, 0.0},
|
|
{0.0, 0.0, 0.0}, 0.0};
|
|
pending_gps = make_position(tick, &stationary_truth, &gps_rng, false);
|
|
pending_gps.position_enu_m[0] *= 0.8 / 1.2;
|
|
pending_gps.position_enu_m[1] *= 0.8 / 1.2;
|
|
pending_gps.position_enu_m[2] *= 1.5 / 2.0;
|
|
pending_gps.covariance_enu_m2[0] = square(0.8);
|
|
pending_gps.covariance_enu_m2[4] = square(0.8);
|
|
pending_gps.covariance_enu_m2[8] = square(1.5);
|
|
delivery_tick = tick + 250000u;
|
|
gps_pending = true;
|
|
next_gps_tick += 1000000u;
|
|
}
|
|
if(gps_pending && (tick >= delivery_tick))
|
|
{
|
|
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
|
CHECK(fusion_engine_push_position(&engine, &pending_gps, &result, NULL),
|
|
"500 Hz delayed stationary GPS processed");
|
|
CHECK(result == ESKF_UPDATE_ACCEPTED,
|
|
"500 Hz stationary GPS passes NIS");
|
|
gps_pending = false;
|
|
}
|
|
if((tick >= 3000000u) && (engine.state == FUSION_ENGINE_RUNNING) &&
|
|
(engine.current.motion.state != FUSION_MOTION_STATIONARY))
|
|
{
|
|
metrics.nonstationary_samples++;
|
|
}
|
|
}
|
|
|
|
metrics.accepted_gps = engine.diagnostics.position_accepted;
|
|
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
|
|
metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
|
|
metrics.covariance_positive_semidefinite = covariance_is_psd(
|
|
&engine.current.filter, NULL);
|
|
metrics.final_horizontal_error_m = norm2(engine.current.filter.position_m[0],
|
|
engine.current.filter.position_m[1]);
|
|
metrics.final_vertical_error_m = fabs(engine.current.filter.position_m[2]);
|
|
metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps);
|
|
return metrics;
|
|
}
|
|
|
|
static void check_stationary_metrics(const stationary_metrics_t* metrics)
|
|
{
|
|
CHECK(metrics->state_finite, "30 s / 500 Hz stationary state remains finite");
|
|
CHECK(metrics->covariance_positive_semidefinite,
|
|
"30 s / 500 Hz stationary covariance remains PSD");
|
|
CHECK(metrics->nonstationary_samples == 0u,
|
|
"30 s / 500 Hz noise never creates a false moving state");
|
|
CHECK(metrics->accepted_gps >= 26u,
|
|
"30 s / 500 Hz delayed stationary GPS remains accepted");
|
|
CHECK(metrics->zero_updates >= 120u,
|
|
"30 s / 500 Hz stationary run keeps PX4-rate ZUPT active");
|
|
CHECK(metrics->final_horizontal_error_m < 1.0,
|
|
"30 s / 500 Hz stationary horizontal error is bounded");
|
|
CHECK(metrics->final_vertical_error_m < 1.0,
|
|
"30 s / 500 Hz stationary vertical error is bounded");
|
|
CHECK(metrics->final_speed_mps < 0.02,
|
|
"30 s / 500 Hz stationary speed remains near zero");
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
synth_metrics_t metrics = run_dynamic_scenario(0u);
|
|
check_dynamic_metrics(&metrics);
|
|
double worst_horizontal_rmse_m = metrics.horizontal_rmse_m;
|
|
double worst_final_horizontal_error_m = metrics.final_horizontal_error_m;
|
|
double latest_stationary_reentry_s = metrics.stationary_reentry_time_s;
|
|
for(uint32_t scenario = 1u ; scenario < 8u ; scenario++)
|
|
{
|
|
synth_metrics_t trial = run_dynamic_scenario(scenario);
|
|
check_dynamic_metrics(&trial);
|
|
worst_horizontal_rmse_m = fmax(worst_horizontal_rmse_m,
|
|
trial.horizontal_rmse_m);
|
|
worst_final_horizontal_error_m = fmax(worst_final_horizontal_error_m,
|
|
trial.final_horizontal_error_m);
|
|
latest_stationary_reentry_s = fmax(latest_stationary_reentry_s,
|
|
trial.stationary_reentry_time_s);
|
|
}
|
|
stationary_metrics_t stationary = run_stationary_500hz_scenario();
|
|
check_stationary_metrics(&stationary);
|
|
printf("Synthetic 60 s asynchronous ESKF scenario\n");
|
|
printf(" position RMSE horizontal=%6.3f m vertical=%6.3f m\n",
|
|
metrics.horizontal_rmse_m, metrics.vertical_rmse_m);
|
|
printf(" final error horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
|
|
metrics.final_horizontal_error_m, metrics.final_vertical_error_m,
|
|
metrics.final_speed_mps);
|
|
printf(" motion timing moving=%6.3f s stationary-again=%6.3f s false-still=%u\n",
|
|
metrics.moving_detection_time_s, metrics.stationary_reentry_time_s,
|
|
metrics.false_stationary_samples);
|
|
printf(" GPS/replay accepted=%llu rejected=%llu replay=%llu replayed-imu=%llu\n",
|
|
(unsigned long long)metrics.accepted_gps,
|
|
(unsigned long long)metrics.rejected_gps,
|
|
(unsigned long long)metrics.replay_count,
|
|
(unsigned long long)metrics.replayed_imu_count);
|
|
printf(" outlier isolation injected=%u rejected=%u max-state-jump=%.3e m\n",
|
|
metrics.injected_outliers, metrics.rejected_injected_outliers,
|
|
metrics.max_rejected_state_jump_m);
|
|
printf(" covariance PSD=%d symmetry-error=%.3e zero-updates=%llu\n",
|
|
metrics.covariance_positive_semidefinite ? 1 : 0,
|
|
metrics.covariance_symmetry_error,
|
|
(unsigned long long)metrics.zero_updates);
|
|
printf(" 8-seed robustness worst-h-rmse=%6.3f m worst-final-h=%6.3f m latest-stop=%6.3f s\n",
|
|
worst_horizontal_rmse_m, worst_final_horizontal_error_m,
|
|
latest_stationary_reentry_s);
|
|
printf("Synthetic 30 s stationary / 500 Hz scenario\n");
|
|
printf(" final error horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
|
|
stationary.final_horizontal_error_m,
|
|
stationary.final_vertical_error_m,
|
|
stationary.final_speed_mps);
|
|
printf(" decisions false-moving-samples=%u GPS-accepted=%llu zero-updates=%llu\n",
|
|
stationary.nonstationary_samples,
|
|
(unsigned long long)stationary.accepted_gps,
|
|
(unsigned long long)stationary.zero_updates);
|
|
if(failures != 0)
|
|
{
|
|
fprintf(stderr, "%d synthetic fusion check(s) failed\n", failures);
|
|
return 1;
|
|
}
|
|
printf("PASS: production bare-metal ESKF sources passed the Linux/GCC scenario.\n");
|
|
return 0;
|
|
}
|