Integrate asynchronous GPS IMU fusion
This commit is contained in:
@@ -0,0 +1,416 @@
|
||||
#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_HZ 52u
|
||||
#define DURATION_S 25.0
|
||||
#define PI 3.14159265358979323846264338327950288
|
||||
#define GPS_EVENT_CAPACITY 300u
|
||||
|
||||
typedef struct {
|
||||
double position_m[3];
|
||||
double velocity_mps[3];
|
||||
double yaw_rad;
|
||||
double accel_nav_mps2[3];
|
||||
double yaw_rate_rps;
|
||||
} truth_t;
|
||||
|
||||
typedef struct {
|
||||
fusion_position_measurement_t measurement;
|
||||
uint64_t arrival_ticks;
|
||||
} gps_event_t;
|
||||
|
||||
typedef struct {
|
||||
unsigned int gps_period_ms;
|
||||
double horizontal_rmse_m;
|
||||
double peak_outage_error_m;
|
||||
double final_horizontal_error_m;
|
||||
double final_vertical_error_m;
|
||||
double final_speed_mps;
|
||||
double outage_sigma_growth_m;
|
||||
double final_horizontal_sigma_m;
|
||||
double maximum_position_step_m;
|
||||
uint64_t gps_accepted;
|
||||
uint64_t gps_rejected;
|
||||
uint64_t replay_count;
|
||||
uint64_t imu_received;
|
||||
unsigned int false_stationary_samples;
|
||||
bool moving_detected;
|
||||
bool recovery_gps_accepted;
|
||||
bool local_state_finite;
|
||||
bool covariance_finite_positive;
|
||||
bool global_origin_available;
|
||||
} metrics_t;
|
||||
|
||||
static bool trace_enabled;
|
||||
|
||||
static double square(double value)
|
||||
{
|
||||
return value * value;
|
||||
}
|
||||
|
||||
static void command_at(double time_s, double accel[3], double* yaw_rate)
|
||||
{
|
||||
memset(accel, 0, 3u * sizeof(double));
|
||||
*yaw_rate = 0.0;
|
||||
if((time_s >= 3.0) && (time_s < 5.0))
|
||||
{
|
||||
accel[0] = 1.0;
|
||||
}
|
||||
else if((time_s >= 8.0) && (time_s < 10.0))
|
||||
{
|
||||
accel[0] = -1.0;
|
||||
}
|
||||
else if((time_s >= 12.0) && (time_s < 14.0))
|
||||
{
|
||||
accel[1] = 0.5;
|
||||
*yaw_rate = PI / 4.0;
|
||||
}
|
||||
else if((time_s >= 18.0) && (time_s < 20.0))
|
||||
{
|
||||
accel[1] = -0.5;
|
||||
}
|
||||
}
|
||||
|
||||
static truth_t truth_at(double time_s)
|
||||
{
|
||||
static const double boundaries[] = {
|
||||
0.0, 3.0, 5.0, 8.0, 10.0, 12.0, 14.0, 18.0, 20.0, DURATION_S
|
||||
};
|
||||
truth_t truth;
|
||||
memset(&truth, 0, sizeof(truth));
|
||||
for(unsigned int segment = 0u;
|
||||
segment + 1u < sizeof(boundaries) / sizeof(boundaries[0]); segment++)
|
||||
{
|
||||
double start = boundaries[segment];
|
||||
if(time_s <= start)
|
||||
{
|
||||
break;
|
||||
}
|
||||
double end = fmin(time_s, boundaries[segment + 1u]);
|
||||
double dt = end - start;
|
||||
double accel[3];
|
||||
double yaw_rate;
|
||||
command_at(start + 0.5 * dt, accel, &yaw_rate);
|
||||
for(unsigned int axis = 0u; axis < 3u; axis++)
|
||||
{
|
||||
truth.position_m[axis] += truth.velocity_mps[axis] * dt +
|
||||
0.5 * accel[axis] * dt * dt;
|
||||
truth.velocity_mps[axis] += accel[axis] * dt;
|
||||
}
|
||||
truth.yaw_rad += yaw_rate * dt;
|
||||
if(end >= time_s)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
command_at(time_s, truth.accel_nav_mps2, &truth.yaw_rate_rps);
|
||||
return truth;
|
||||
}
|
||||
|
||||
static fusion_imu_measurement_t make_imu(uint64_t ticks, double gravity)
|
||||
{
|
||||
double time_s = (double)ticks / (double)TIMEBASE_HZ;
|
||||
truth_t truth = truth_at(time_s);
|
||||
double cosine = cos(truth.yaw_rad);
|
||||
double sine = sin(truth.yaw_rad);
|
||||
double specific_force_nav[3] = {
|
||||
truth.accel_nav_mps2[0], truth.accel_nav_mps2[1],
|
||||
truth.accel_nav_mps2[2] + gravity
|
||||
};
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = ticks;
|
||||
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] = truth.yaw_rate_rps;
|
||||
/* Constant offsets are learned during stationary startup. Small
|
||||
* deterministic broadband components keep the test reproducible. */
|
||||
static const double accel_bias[3] = {0.030, -0.020, 0.050};
|
||||
static const double gyro_bias[3] = {0.0008, -0.0005, 0.0006};
|
||||
for(unsigned int axis = 0u; axis < 3u; axis++)
|
||||
{
|
||||
double phase = time_s * (11.0 + 2.0 * axis) + (double)axis;
|
||||
imu.accel_mps2[axis] += accel_bias[axis] + 0.008 * sin(phase);
|
||||
imu.gyro_rps[axis] += gyro_bias[axis] + 0.00025 * cos(1.7 * phase);
|
||||
}
|
||||
imu.temperature_c = 25.0 + 0.4 * sin(0.1 * time_s);
|
||||
imu.sequence = (uint32_t)((ticks * IMU_HZ + TIMEBASE_HZ / 2u) /
|
||||
TIMEBASE_HZ);
|
||||
imu.vehicle_at_rest =
|
||||
(hypot(truth.velocity_mps[0], truth.velocity_mps[1]) < 0.05) &&
|
||||
(hypot(truth.accel_nav_mps2[0], truth.accel_nav_mps2[1]) < 0.05) &&
|
||||
(fabs(truth.yaw_rate_rps) < 0.02);
|
||||
return imu;
|
||||
}
|
||||
|
||||
static fusion_position_measurement_t make_gps(uint64_t ticks)
|
||||
{
|
||||
double time_s = (double)ticks / (double)TIMEBASE_HZ;
|
||||
truth_t truth = truth_at(time_s);
|
||||
fusion_position_measurement_t gps;
|
||||
memset(&gps, 0, sizeof(gps));
|
||||
gps.timestamp_ticks = ticks;
|
||||
gps.position_enu_m[0] = truth.position_m[0] + 0.45 * sin(7.1 * time_s);
|
||||
gps.position_enu_m[1] = truth.position_m[1] + 0.45 * cos(5.3 * time_s);
|
||||
gps.position_enu_m[2] = truth.position_m[2] + 0.70 * sin(3.7 * time_s);
|
||||
gps.covariance_enu_m2[0] = square(1.2);
|
||||
gps.covariance_enu_m2[4] = square(1.2);
|
||||
gps.covariance_enu_m2[8] = square(2.0);
|
||||
gps.source_quality = 1.0;
|
||||
gps.source_valid = true;
|
||||
return gps;
|
||||
}
|
||||
|
||||
static bool covariance_diagonal_valid(const eskf_t* filter)
|
||||
{
|
||||
for(unsigned int index = 0u; index < ESKF_ERROR_STATE_DIM; index++)
|
||||
{
|
||||
if(!isfinite(filter->covariance[index][index]) ||
|
||||
!(filter->covariance[index][index] > 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static metrics_t run_scenario(unsigned int gps_period_ms)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
memset(&engine, 0, sizeof(engine));
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, TIMEBASE_HZ);
|
||||
config.calibration_min_samples = 100u;
|
||||
metrics_t metrics;
|
||||
memset(&metrics, 0, sizeof(metrics));
|
||||
metrics.gps_period_ms = gps_period_ms;
|
||||
if(!fusion_engine_init(&engine, &config))
|
||||
{
|
||||
return metrics;
|
||||
}
|
||||
if(gps_period_ms != 0u)
|
||||
{
|
||||
fusion_position_measurement_t origin = make_gps(0u);
|
||||
origin.position_enu_m[0] = 0.0;
|
||||
origin.position_enu_m[1] = 0.0;
|
||||
origin.position_enu_m[2] = 0.0;
|
||||
(void)fusion_engine_push_position(&engine, &origin, NULL, NULL);
|
||||
}
|
||||
|
||||
gps_event_t events[GPS_EVENT_CAPACITY];
|
||||
unsigned int event_head = 0u;
|
||||
unsigned int event_count = 0u;
|
||||
uint64_t next_gps_ticks = 3037000u;
|
||||
uint64_t gps_period_ticks = (uint64_t)gps_period_ms * 1000u;
|
||||
double error_square_sum = 0.0;
|
||||
uint64_t error_samples = 0u;
|
||||
double sigma_at_outage_start = NAN;
|
||||
double sigma_before_recovery = NAN;
|
||||
uint64_t accepted_before_recovery = 0u;
|
||||
double previous_position[3] = {0.0, 0.0, 0.0};
|
||||
bool have_previous_position = false;
|
||||
uint64_t sample_count = (uint64_t)(DURATION_S * (double)IMU_HZ);
|
||||
|
||||
for(uint64_t sample = 0u; sample <= sample_count; sample++)
|
||||
{
|
||||
uint64_t ticks = (sample * TIMEBASE_HZ + IMU_HZ / 2u) / IMU_HZ;
|
||||
double time_s = (double)ticks / (double)TIMEBASE_HZ;
|
||||
fusion_imu_measurement_t imu = make_imu(ticks, config.eskf.gravity_mps2);
|
||||
if(gps_period_ms == 0u)
|
||||
{
|
||||
/* Preserve the pure unaided observability case: no GNSS and no
|
||||
* external platform-rest constraint. */
|
||||
imu.vehicle_at_rest = false;
|
||||
}
|
||||
if(trace_enabled)
|
||||
{
|
||||
printf("IMU ts=%llu seq=%u accel=%.6f,%.6f,%.6f gyro=%.6f,%.6f,%.6f\n",
|
||||
(unsigned long long)imu.timestamp_ticks, imu.sequence,
|
||||
imu.accel_mps2[0], imu.accel_mps2[1], imu.accel_mps2[2],
|
||||
imu.gyro_rps[0], imu.gyro_rps[1], imu.gyro_rps[2]);
|
||||
}
|
||||
if(!fusion_engine_push_imu(&engine, &imu))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if((gps_period_ms != 0u) && (ticks >= next_gps_ticks))
|
||||
{
|
||||
double gps_time_s = (double)next_gps_ticks / (double)TIMEBASE_HZ;
|
||||
if(!((gps_time_s >= 10.0) && (gps_time_s < 18.0)) &&
|
||||
(event_count < GPS_EVENT_CAPACITY))
|
||||
{
|
||||
events[event_count].measurement = make_gps(next_gps_ticks);
|
||||
events[event_count].arrival_ticks = next_gps_ticks +
|
||||
((gps_period_ms == 1000u) ? 310000u : 73000u);
|
||||
event_count++;
|
||||
}
|
||||
next_gps_ticks += gps_period_ticks;
|
||||
}
|
||||
|
||||
while((event_head < event_count) &&
|
||||
(events[event_head].arrival_ticks <= ticks))
|
||||
{
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
double nis = NAN;
|
||||
(void)fusion_engine_push_position(
|
||||
&engine, &events[event_head].measurement, &result, &nis);
|
||||
if(trace_enabled)
|
||||
{
|
||||
printf("GPS ts=%llu arrival=%llu result=%u nis=%.6f pos=%.3f,%.3f,%.3f\n",
|
||||
(unsigned long long)events[event_head].measurement.timestamp_ticks,
|
||||
(unsigned long long)ticks, (unsigned int)result, nis,
|
||||
events[event_head].measurement.position_enu_m[0],
|
||||
events[event_head].measurement.position_enu_m[1],
|
||||
events[event_head].measurement.position_enu_m[2]);
|
||||
}
|
||||
event_head++;
|
||||
}
|
||||
|
||||
if(engine.state == FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
truth_t truth = truth_at(time_s);
|
||||
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 horizontal_error = hypot(east_error, north_error);
|
||||
if(have_previous_position)
|
||||
{
|
||||
double position_step = hypot(
|
||||
engine.current.filter.position_m[0] - previous_position[0],
|
||||
engine.current.filter.position_m[1] - previous_position[1]);
|
||||
metrics.maximum_position_step_m =
|
||||
fmax(metrics.maximum_position_step_m, position_step);
|
||||
}
|
||||
memcpy(previous_position, engine.current.filter.position_m,
|
||||
sizeof(previous_position));
|
||||
have_previous_position = true;
|
||||
if(time_s >= 2.5)
|
||||
{
|
||||
error_square_sum += square(horizontal_error);
|
||||
error_samples++;
|
||||
}
|
||||
if((time_s >= 10.0) && (time_s < 18.0))
|
||||
{
|
||||
metrics.peak_outage_error_m =
|
||||
fmax(metrics.peak_outage_error_m, horizontal_error);
|
||||
}
|
||||
if((hypot(truth.velocity_mps[0], truth.velocity_mps[1]) > 0.2) &&
|
||||
(engine.current.motion.state == FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
metrics.false_stationary_samples++;
|
||||
}
|
||||
if(engine.current.motion.state == FUSION_MOTION_MOVING)
|
||||
{
|
||||
metrics.moving_detected = true;
|
||||
}
|
||||
if(isnan(sigma_at_outage_start) && (time_s >= 10.0))
|
||||
{
|
||||
sigma_at_outage_start = sqrt(engine.current.filter.covariance[0][0]);
|
||||
accepted_before_recovery = engine.diagnostics.position_accepted;
|
||||
}
|
||||
if(isnan(sigma_before_recovery) && (time_s >= 18.0))
|
||||
{
|
||||
sigma_before_recovery = sqrt(engine.current.filter.covariance[0][0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
truth_t final_truth = truth_at(DURATION_S);
|
||||
metrics.horizontal_rmse_m = (error_samples != 0u)
|
||||
? sqrt(error_square_sum / (double)error_samples) : INFINITY;
|
||||
metrics.final_horizontal_error_m = hypot(
|
||||
engine.current.filter.position_m[0] - final_truth.position_m[0],
|
||||
engine.current.filter.position_m[1] - final_truth.position_m[1]);
|
||||
metrics.final_vertical_error_m =
|
||||
fabs(engine.current.filter.position_m[2] - final_truth.position_m[2]);
|
||||
metrics.final_speed_mps = eskf_speed_mps(&engine.current.filter);
|
||||
metrics.outage_sigma_growth_m =
|
||||
(isfinite(sigma_at_outage_start) && isfinite(sigma_before_recovery))
|
||||
? sigma_before_recovery - sigma_at_outage_start : 0.0;
|
||||
metrics.final_horizontal_sigma_m = sqrt(
|
||||
engine.current.filter.covariance[0][0] +
|
||||
engine.current.filter.covariance[1][1]);
|
||||
metrics.gps_accepted = engine.diagnostics.position_accepted;
|
||||
metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
|
||||
metrics.replay_count = engine.diagnostics.replay_count;
|
||||
metrics.imu_received = engine.diagnostics.imu_received;
|
||||
metrics.recovery_gps_accepted = (gps_period_ms == 0u) ||
|
||||
(engine.diagnostics.position_accepted > accepted_before_recovery);
|
||||
metrics.local_state_finite = (engine.state == FUSION_ENGINE_RUNNING) &&
|
||||
eskf_state_is_finite(&engine.current.filter);
|
||||
metrics.covariance_finite_positive =
|
||||
covariance_diagonal_valid(&engine.current.filter);
|
||||
metrics.global_origin_available = engine.have_initial_position;
|
||||
return metrics;
|
||||
}
|
||||
|
||||
static void print_metrics(const metrics_t* metrics)
|
||||
{
|
||||
printf("GPS=%4u ms IMU=%llu GPS-ok/rej=%llu/%llu replay=%llu "
|
||||
"rmse-h=%.3f m outage-peak=%.3f m sigma-growth=%.3f m "
|
||||
"final=%.3f/%.3f m sigma-h=%.3f m step-max=%.3f m "
|
||||
"speed=%.4f m/s false-ZUPT=%u origin=%u\n",
|
||||
metrics->gps_period_ms,
|
||||
(unsigned long long)metrics->imu_received,
|
||||
(unsigned long long)metrics->gps_accepted,
|
||||
(unsigned long long)metrics->gps_rejected,
|
||||
(unsigned long long)metrics->replay_count,
|
||||
metrics->horizontal_rmse_m, metrics->peak_outage_error_m,
|
||||
metrics->outage_sigma_growth_m,
|
||||
metrics->final_horizontal_error_m, metrics->final_vertical_error_m,
|
||||
metrics->final_horizontal_sigma_m, metrics->maximum_position_step_m,
|
||||
metrics->final_speed_mps, metrics->false_stationary_samples,
|
||||
metrics->global_origin_available ? 1u : 0u);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
trace_enabled = (argc == 2) && (strcmp(argv[1], "--trace") == 0);
|
||||
const unsigned int periods_ms[] = {100u, 200u, 1000u, 0u};
|
||||
bool passed = true;
|
||||
printf("Timestamped 52 Hz IMU, asynchronous GPS, 8 s outage (10..18 s)\n");
|
||||
for(unsigned int index = 0u;
|
||||
index < sizeof(periods_ms) / sizeof(periods_ms[0]); index++)
|
||||
{
|
||||
metrics_t metrics = run_scenario(periods_ms[index]);
|
||||
print_metrics(&metrics);
|
||||
bool common_ok = metrics.local_state_finite &&
|
||||
metrics.covariance_finite_positive &&
|
||||
metrics.moving_detected &&
|
||||
(metrics.false_stationary_samples == 0u) &&
|
||||
(metrics.imu_received == 1301u) &&
|
||||
(metrics.maximum_position_step_m < 1.0);
|
||||
bool gps_ok = (periods_ms[index] == 0u)
|
||||
? (!metrics.global_origin_available && (metrics.gps_accepted == 0u) &&
|
||||
(metrics.peak_outage_error_m < 1.5) &&
|
||||
(metrics.final_horizontal_error_m < 5.0) &&
|
||||
(metrics.final_horizontal_error_m <
|
||||
3.0 * metrics.final_horizontal_sigma_m) &&
|
||||
(metrics.outage_sigma_growth_m > 0.0))
|
||||
: (metrics.global_origin_available && metrics.recovery_gps_accepted &&
|
||||
(metrics.gps_rejected == 0u) && (metrics.replay_count > 0u) &&
|
||||
(metrics.peak_outage_error_m < 2.0) &&
|
||||
(metrics.outage_sigma_growth_m > 0.0) &&
|
||||
(metrics.final_speed_mps < 0.08) &&
|
||||
(metrics.final_horizontal_error_m < 1.5) &&
|
||||
(metrics.final_vertical_error_m < 1.5));
|
||||
passed = passed && common_ok && gps_ok;
|
||||
}
|
||||
if(!passed)
|
||||
{
|
||||
fprintf(stderr, "FAIL: asynchronous rate/dead-reckoning matrix.\n");
|
||||
return 1;
|
||||
}
|
||||
printf("PASS: all rates use the same IMU prediction path; GPS loss remains "
|
||||
"local dead reckoning and recovery is an external update.\n");
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user