316 lines
10 KiB
C
316 lines
10 KiB
C
#include "fusion/fusion_engine.h"
|
|
|
|
#include <math.h>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#define TEST_TIMEBASE_HZ 1000000u
|
|
#define TEST_IMU_HZ 52u
|
|
#define TEST_DURATION_S 30u
|
|
#define TEST_PI 3.14159265358979323846264338327950288
|
|
#define GPS_PERIOD_TICKS 200000u
|
|
#define GPS_DELAY_TICKS 73000u
|
|
#define MAX_GPS_EVENTS 160u
|
|
|
|
typedef struct {
|
|
fusion_position_measurement_t measurement;
|
|
uint64_t arrival_ticks;
|
|
} gps_event_t;
|
|
|
|
typedef struct {
|
|
const char* name;
|
|
double peak_horizontal_error_m;
|
|
double peak_vertical_error_m;
|
|
double final_horizontal_error_m;
|
|
double final_vertical_error_m;
|
|
double final_speed_mps;
|
|
double final_roll_error_deg;
|
|
uint64_t imu_pushed;
|
|
uint64_t gps_accepted;
|
|
uint64_t gps_rejected;
|
|
uint64_t zero_updates;
|
|
fusion_engine_state_t engine_state;
|
|
fusion_motion_state_t motion_state;
|
|
} scenario_metrics_t;
|
|
|
|
static double radians(double degrees)
|
|
{
|
|
return degrees * TEST_PI / 180.0;
|
|
}
|
|
|
|
static double degrees(double radians_value)
|
|
{
|
|
return radians_value * 180.0 / TEST_PI;
|
|
}
|
|
|
|
/* The IMU rotates about its own X axis without translating. Calibration is
|
|
* complete before the manoeuvre. This is the smallest synthetic version of
|
|
* the live hand-tilt failure: the true position and the GNSS antenna remain at
|
|
* the origin while body-frame gravity changes with roll. */
|
|
static void truth_roll(uint64_t ticks,
|
|
double final_roll_rad,
|
|
double* roll_rad,
|
|
double* roll_rate_rps)
|
|
{
|
|
double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
|
|
double roll = 0.0;
|
|
double rate = 0.0;
|
|
const double maximum_roll = radians(20.0);
|
|
/* A short hand motion is intentionally placed between the 250 ms selected
|
|
* samples. Full-rate IMU observes the angular rate; sample dropping can
|
|
* observe only the changed gravity vector and completely miss the gyro. */
|
|
if((time_s >= 16.05) && (time_s < 16.15))
|
|
{
|
|
roll = maximum_roll * (time_s - 16.05) / 0.10;
|
|
rate = maximum_roll / 0.10;
|
|
}
|
|
else if((time_s >= 16.15) && (time_s < 17.05))
|
|
{
|
|
roll = maximum_roll;
|
|
}
|
|
else if((time_s >= 17.05) && (time_s < 17.15))
|
|
{
|
|
double fraction = (time_s - 17.05) / 0.10;
|
|
roll = maximum_roll + fraction * (final_roll_rad - maximum_roll);
|
|
rate = (final_roll_rad - maximum_roll) / 0.10;
|
|
}
|
|
else if(time_s >= 17.15)
|
|
{
|
|
roll = final_roll_rad;
|
|
}
|
|
*roll_rad = roll;
|
|
*roll_rate_rps = rate;
|
|
}
|
|
|
|
static fusion_imu_measurement_t make_imu(uint64_t ticks,
|
|
double gravity_mps2,
|
|
double final_roll_rad,
|
|
double gyro_scale)
|
|
{
|
|
double roll;
|
|
double roll_rate;
|
|
truth_roll(ticks, final_roll_rad, &roll, &roll_rate);
|
|
fusion_imu_measurement_t imu;
|
|
memset(&imu, 0, sizeof(imu));
|
|
imu.timestamp_ticks = ticks;
|
|
/* R_nb is a positive X rotation. A stationary accelerometer measures
|
|
* R_nb^T [0, 0, g], hence [0, g sin(phi), g cos(phi)] in body axes. */
|
|
imu.accel_mps2[1] = gravity_mps2 * sin(roll);
|
|
imu.accel_mps2[2] = gravity_mps2 * cos(roll);
|
|
imu.gyro_rps[0] = gyro_scale * roll_rate;
|
|
imu.temperature_c = 25.0;
|
|
imu.sequence = (uint32_t)((ticks * TEST_IMU_HZ + TEST_TIMEBASE_HZ / 2u) /
|
|
TEST_TIMEBASE_HZ);
|
|
double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
|
|
imu.vehicle_at_rest = (time_s < 16.05) || (time_s >= 18.15);
|
|
return imu;
|
|
}
|
|
|
|
static fusion_position_measurement_t make_gps(uint64_t ticks)
|
|
{
|
|
fusion_position_measurement_t gps;
|
|
memset(&gps, 0, sizeof(gps));
|
|
gps.timestamp_ticks = ticks;
|
|
gps.covariance_enu_m2[0] = 4.0;
|
|
gps.covariance_enu_m2[4] = 4.0;
|
|
gps.covariance_enu_m2[8] = 9.0;
|
|
gps.source_quality = 100.0;
|
|
gps.source_valid = true;
|
|
return gps;
|
|
}
|
|
|
|
static unsigned int build_gps_events(gps_event_t events[MAX_GPS_EVENTS])
|
|
{
|
|
unsigned int count = 0u;
|
|
/* Deliberately offset GNSS epochs from both the 52 Hz IMU samples and
|
|
* integer seconds. */
|
|
for(uint64_t ticks = 123457u;
|
|
ticks <= (uint64_t)TEST_DURATION_S * TEST_TIMEBASE_HZ;
|
|
ticks += GPS_PERIOD_TICKS)
|
|
{
|
|
events[count].measurement = make_gps(ticks);
|
|
events[count].arrival_ticks = ticks + GPS_DELAY_TICKS;
|
|
count++;
|
|
}
|
|
return count;
|
|
}
|
|
|
|
static double quaternion_roll(const double quaternion[4])
|
|
{
|
|
double w = quaternion[0];
|
|
double x = quaternion[1];
|
|
double y = quaternion[2];
|
|
double z = quaternion[3];
|
|
return atan2(2.0 * (w * x + y * z),
|
|
1.0 - 2.0 * (x * x + y * y));
|
|
}
|
|
|
|
static scenario_metrics_t run_scenario(const char* name,
|
|
bool emulate_250_ms_sample_drop,
|
|
bool provide_gps,
|
|
double final_roll_deg,
|
|
double gyro_scale)
|
|
{
|
|
static fusion_engine_t engine;
|
|
memset(&engine, 0, sizeof(engine));
|
|
fusion_engine_config_t config;
|
|
fusion_engine_default_config(&config, TEST_TIMEBASE_HZ);
|
|
if(!fusion_engine_init(&engine, &config))
|
|
{
|
|
return (scenario_metrics_t){.name = name, .engine_state = FUSION_ENGINE_FAULT};
|
|
}
|
|
|
|
/* Give calibration an absolute origin, just as the product does when GNSS
|
|
* becomes available before initial tilt/bias calibration completes. */
|
|
if(provide_gps)
|
|
{
|
|
fusion_position_measurement_t origin = make_gps(1u);
|
|
(void)fusion_engine_push_position(&engine, &origin, NULL, NULL);
|
|
}
|
|
|
|
gps_event_t events[MAX_GPS_EVENTS];
|
|
unsigned int event_count = provide_gps ? build_gps_events(events) : 0u;
|
|
unsigned int event_head = 0u;
|
|
uint64_t last_forwarded_ticks = 0u;
|
|
bool have_forwarded = false;
|
|
scenario_metrics_t metrics;
|
|
memset(&metrics, 0, sizeof(metrics));
|
|
metrics.name = name;
|
|
|
|
const uint64_t sample_count =
|
|
(uint64_t)TEST_DURATION_S * TEST_IMU_HZ;
|
|
for(uint64_t sample = 0u ; sample <= sample_count ; sample++)
|
|
{
|
|
uint64_t ticks = (sample * TEST_TIMEBASE_HZ + TEST_IMU_HZ / 2u) /
|
|
TEST_IMU_HZ;
|
|
fusion_imu_measurement_t imu = make_imu(
|
|
ticks, config.eskf.gravity_mps2, radians(final_roll_deg), gyro_scale);
|
|
bool forward = true;
|
|
if(emulate_250_ms_sample_drop && have_forwarded &&
|
|
((ticks - last_forwarded_ticks) < 250000u))
|
|
{
|
|
forward = false;
|
|
}
|
|
if(forward)
|
|
{
|
|
if(!fusion_engine_push_imu(&engine, &imu))
|
|
{
|
|
break;
|
|
}
|
|
last_forwarded_ticks = ticks;
|
|
have_forwarded = true;
|
|
metrics.imu_pushed++;
|
|
}
|
|
|
|
while((event_head < event_count) &&
|
|
(events[event_head].arrival_ticks <= ticks))
|
|
{
|
|
(void)fusion_engine_push_position(
|
|
&engine, &events[event_head].measurement, NULL, NULL);
|
|
event_head++;
|
|
}
|
|
|
|
if(engine.state == FUSION_ENGINE_RUNNING)
|
|
{
|
|
double horizontal = hypot(engine.current.filter.position_m[0],
|
|
engine.current.filter.position_m[1]);
|
|
metrics.peak_horizontal_error_m =
|
|
fmax(metrics.peak_horizontal_error_m, horizontal);
|
|
metrics.peak_vertical_error_m =
|
|
fmax(metrics.peak_vertical_error_m,
|
|
fabs(engine.current.filter.position_m[2]));
|
|
}
|
|
}
|
|
|
|
metrics.engine_state = engine.state;
|
|
if(engine.state == FUSION_ENGINE_RUNNING)
|
|
{
|
|
metrics.final_horizontal_error_m =
|
|
hypot(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 = eskf_speed_mps(&engine.current.filter);
|
|
metrics.final_roll_error_deg =
|
|
fabs(degrees(quaternion_roll(engine.current.filter.body_to_nav_quaternion)) -
|
|
final_roll_deg);
|
|
metrics.motion_state = engine.current.motion.state;
|
|
}
|
|
metrics.gps_accepted = engine.diagnostics.position_accepted;
|
|
metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
|
|
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
|
|
return metrics;
|
|
}
|
|
|
|
static void print_metrics(const scenario_metrics_t* metrics)
|
|
{
|
|
printf("%-20s imu=%4llu state=%u motion=%-20s peak-h=%9.3f m peak-v=%9.3f m "
|
|
"final-h=%9.3f m final-v=%9.3f m speed=%8.3f m/s "
|
|
"roll=%7.3f deg gps=%llu/%llu zupt=%llu\n",
|
|
metrics->name,
|
|
(unsigned long long)metrics->imu_pushed,
|
|
(unsigned int)metrics->engine_state,
|
|
fusion_motion_state_name(metrics->motion_state),
|
|
metrics->peak_horizontal_error_m,
|
|
metrics->peak_vertical_error_m,
|
|
metrics->final_horizontal_error_m,
|
|
metrics->final_vertical_error_m,
|
|
metrics->final_speed_mps,
|
|
metrics->final_roll_error_deg,
|
|
(unsigned long long)metrics->gps_accepted,
|
|
(unsigned long long)metrics->gps_rejected,
|
|
(unsigned long long)metrics->zero_updates);
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
scenario_metrics_t full_rate = run_scenario(
|
|
"52 Hz full IMU", false, true, 0.0, 1.0);
|
|
scenario_metrics_t imu_only = run_scenario(
|
|
"52 Hz, no GNSS", false, false, 0.0, 1.0);
|
|
scenario_metrics_t residual = run_scenario(
|
|
"1 deg residual tilt", false, true, 10.0, 0.9);
|
|
scenario_metrics_t sparse = run_scenario(
|
|
"250 ms sample-drop", true, true, 0.0, 1.0);
|
|
printf("Asynchronous stationary tilt: 52 Hz IMU, 5 Hz GNSS, 73 ms GNSS delay\n");
|
|
print_metrics(&full_rate);
|
|
print_metrics(&imu_only);
|
|
print_metrics(&residual);
|
|
print_metrics(&sparse);
|
|
|
|
bool full_rate_ok = (full_rate.engine_state == FUSION_ENGINE_RUNNING) &&
|
|
(full_rate.peak_horizontal_error_m < 1.0) &&
|
|
(full_rate.peak_vertical_error_m < 1.0) &&
|
|
(full_rate.final_speed_mps < 0.05);
|
|
bool imu_only_ok = (imu_only.engine_state == FUSION_ENGINE_RUNNING) &&
|
|
(imu_only.gps_accepted == 0u) &&
|
|
(imu_only.peak_horizontal_error_m < 1.0) &&
|
|
(imu_only.peak_vertical_error_m < 1.0) &&
|
|
(imu_only.final_speed_mps < 0.05);
|
|
bool residual_recovered =
|
|
(residual.engine_state == FUSION_ENGINE_RUNNING) &&
|
|
(residual.motion_state == FUSION_MOTION_STATIONARY) &&
|
|
(residual.final_speed_mps < 0.05) &&
|
|
(residual.final_horizontal_error_m < 1.0) &&
|
|
(residual.final_roll_error_deg < 0.5);
|
|
bool sparse_aliasing_detected_and_recovered =
|
|
(sparse.engine_state == FUSION_ENGINE_RUNNING) &&
|
|
(sparse.peak_horizontal_error_m > 3.0) &&
|
|
(sparse.final_horizontal_error_m < 1.0) &&
|
|
(sparse.final_speed_mps < 0.10) &&
|
|
(sparse.motion_state == FUSION_MOTION_STATIONARY);
|
|
if(!full_rate_ok || !imu_only_ok || !residual_recovered ||
|
|
!sparse_aliasing_detected_and_recovered)
|
|
{
|
|
fprintf(stderr,
|
|
"FAIL: full-rate propagation and GNSS-aided rest recovery must "
|
|
"remain bounded; sparse sampling must expose a large transient but "
|
|
"recover after an external at-rest decision.\n");
|
|
return 1;
|
|
}
|
|
printf("PASS: full-rate and residual-tilt cases remained bounded; sparse "
|
|
"sampling exposed the transient and external at-rest recovered it.\n");
|
|
return 0;
|
|
}
|