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

815 lines
28 KiB
C
Raw Blame History

#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include "fusion/eskf.h"
#include "fusion/fusion_engine.h"
#include "fusion/fusion_bus.h"
#include "fusion/fusion_gps_adapter.h"
#include "fusion/fusion_motion.h"
#define TEST_PI 3.14159265358979323846264338327950288
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
static bool nearly_equal(double first, double second, double tolerance)
{
return fabs(first - second) <= tolerance;
}
static double test_norm3(const double value[3])
{
return sqrt(value[0] * value[0] + value[1] * value[1] +
value[2] * value[2]);
}
static uint32_t noise_state = 0x9e3779b9u;
static double uniform_noise(void)
{
noise_state ^= noise_state << 13;
noise_state ^= noise_state >> 17;
noise_state ^= noise_state << 5;
return ((double)noise_state + 0.5) / 4294967296.0;
}
static double gaussian_noise(void)
{
double first = fmax(uniform_noise(), 1.0e-12);
double second = uniform_noise();
return sqrt(-2.0 * log(first)) * cos(2.0 * TEST_PI * second);
}
static void identity_filter(eskf_t* filter, eskf_config_t* config)
{
eskf_default_config(config);
double position[3] = {0.0, 0.0, 0.0};
double quaternion[4] = {1.0, 0.0, 0.0, 0.0};
double accel_bias[3] = {0.0, 0.0, 0.0};
double gyro_bias[3] = {0.0, 0.0, 0.0};
CHECK(eskf_initialize(filter, config, position, quaternion, accel_bias, gyro_bias),
"ESKF identity initialization");
}
static void test_stationary_propagation(void)
{
eskf_t filter;
eskf_config_t config;
identity_filter(&filter, &config);
double accel[3] = {0.0, 0.0, config.gravity_mps2};
double gyro[3] = {0.0, 0.0, 0.0};
for(unsigned int i = 0 ; i < 500u ; i++)
{
CHECK(eskf_propagate(&filter, &config, accel, gyro, 0.01),
"stationary propagation succeeds");
}
CHECK(fabs(eskf_speed_mps(&filter)) < 1.0e-9,
"stationary propagation keeps zero velocity");
CHECK(fabs(filter.position_m[2]) < 1.0e-9,
"stationary propagation keeps zero height");
CHECK(eskf_state_is_finite(&filter), "stationary state remains finite");
}
static void test_zero_update_primitives(void)
{
eskf_t filter;
eskf_config_t config;
identity_filter(&filter, &config);
filter.velocity_mps[0] = 1.2;
filter.velocity_mps[1] = -0.8;
filter.velocity_mps[2] = 0.3;
CHECK(eskf_zero_velocity_update(&filter, 0.03),
"direct ZUPT measurement update succeeds");
CHECK(eskf_speed_mps(&filter) < 0.005,
"direct ZUPT drives a stale velocity estimate close to zero");
double stationary_gyro[3] = {0.0010, -0.0006, 0.0003};
double gyro_std[3] = {0.0002, 0.0002, 0.0002};
for(unsigned int update = 0u ; update < 20u ; update++)
{
CHECK(eskf_zero_angular_rate_update(
&filter, stationary_gyro, gyro_std),
"direct zero-angular-rate update succeeds");
}
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
CHECK(fabs(filter.gyro_bias_rps[axis] - stationary_gyro[axis]) < 1.0e-5,
"zero-angular-rate update converges gyro bias to stationary reading");
}
CHECK(eskf_state_is_finite(&filter),
"ZUPT and zero-angular-rate updates preserve finite ESKF state");
for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
{
CHECK(filter.covariance[row][row] > 0.0,
"zero updates preserve positive covariance diagonal");
for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
{
CHECK(fabs(filter.covariance[row][col] -
filter.covariance[col][row]) < 1.0e-12,
"zero updates preserve covariance symmetry");
}
}
}
static double quaternion_yaw(const double quaternion[4])
{
return atan2(
2.0 * (quaternion[0] * quaternion[3] +
quaternion[1] * quaternion[2]),
1.0 - 2.0 * (quaternion[2] * quaternion[2] +
quaternion[3] * quaternion[3]));
}
static void test_external_rest_cannot_erase_constant_turn(void)
{
fusion_engine_config_t config;
fusion_engine_t engine;
fusion_engine_default_config(&config, 1000000u);
CHECK(fusion_engine_init(&engine, &config),
"constant-turn engine initialization");
for(unsigned int sample = 0u ; sample <= 8u * 52u ; sample++)
{
uint64_t ticks = ((uint64_t)sample * 1000000u + 26u) / 52u;
double time_s = (double)ticks / 1000000.0;
fusion_imu_measurement_t imu;
memset(&imu, 0, sizeof(imu));
imu.timestamp_ticks = ticks;
imu.accel_mps2[2] = config.eskf.gravity_mps2;
imu.gyro_rps[2] =
(time_s >= 3.0) ? 20.0 * TEST_PI / 180.0 : 0.0;
imu.temperature_c = 25.0;
/* Models a stale/incorrect external landed flag. Inertial motion must
* still veto ZUPT and zero-gyro fusion. */
imu.vehicle_at_rest = true;
CHECK(fusion_engine_push_imu(&engine, &imu),
"constant-turn IMU accepted");
}
CHECK(engine.state == FUSION_ENGINE_RUNNING,
"constant-turn engine remains running");
double yaw_deg = quaternion_yaw(
engine.current.filter.body_to_nav_quaternion) * 180.0 / TEST_PI;
CHECK(fabs(yaw_deg - 100.0) < 1.0,
"stale external rest flag cannot erase a real constant turn");
CHECK(fabs(engine.current.filter.gyro_bias_rps[2]) <
0.5 * TEST_PI / 180.0,
"constant angular rate is not learned as gyro bias");
}
static void test_gravity_alignment(void)
{
eskf_t filter;
eskf_config_t config;
eskf_default_config(&config);
double measured_gravity[3] = {0.35, -0.22, 9.79793};
double nav_up[3] = {0.0, 0.0, 1.0};
double quaternion[4];
CHECK(eskf_quaternion_align_vectors(measured_gravity, nav_up, quaternion),
"gravity alignment quaternion");
double position[3] = {0.0, 0.0, 0.0};
double accel_bias[3] = {0.0, 0.0, 0.0};
double gyro_bias[3] = {0.0, 0.0, 0.0};
double gravity_norm = sqrt(measured_gravity[0] * measured_gravity[0] +
measured_gravity[1] * measured_gravity[1] +
measured_gravity[2] * measured_gravity[2]);
for(unsigned int i = 0 ; i < 3u ; i++)
{
measured_gravity[i] *= config.gravity_mps2 / gravity_norm;
}
CHECK(eskf_initialize(&filter, &config, position, quaternion,
accel_bias, gyro_bias),
"tilted ESKF initialization");
double gyro[3] = {0.0, 0.0, 0.0};
CHECK(eskf_propagate(&filter, &config, measured_gravity, gyro, 0.01),
"tilted stationary propagation");
CHECK(sqrt(filter.last_linear_accel_nav_mps2[0] *
filter.last_linear_accel_nav_mps2[0] +
filter.last_linear_accel_nav_mps2[1] *
filter.last_linear_accel_nav_mps2[1] +
filter.last_linear_accel_nav_mps2[2] *
filter.last_linear_accel_nav_mps2[2]) < 1.0e-8,
"gravity alignment removes gravity from linear acceleration");
}
static void test_position_nis_gate(void)
{
eskf_t filter;
eskf_config_t config;
identity_filter(&filter, &config);
double covariance[9] = {
4.0, 0.2, 0.0,
0.2, 4.0, 0.0,
0.0, 0.0, 9.0
};
double normal_position[3] = {1.0, -0.5, 0.2};
double nis = NAN;
CHECK(eskf_position_update(&filter, &config, normal_position,
covariance, &nis) == ESKF_UPDATE_ACCEPTED,
"normal GPS position accepted");
CHECK(isfinite(nis) && (nis < config.position_nis_gate),
"accepted GPS has finite in-gate NIS");
double before[3];
memcpy(before, filter.position_m, sizeof(before));
double outlier[3] = {1000.0, -800.0, 500.0};
CHECK(eskf_position_update(&filter, &config, outlier,
covariance, &nis) == ESKF_UPDATE_REJECTED_NIS,
"large GPS outlier rejected by NIS");
CHECK(nearly_equal(before[0], filter.position_m[0], 1.0e-12) &&
nearly_equal(before[1], filter.position_m[1], 1.0e-12) &&
nearly_equal(before[2], filter.position_m[2], 1.0e-12),
"NIS rejection leaves nominal position unchanged");
CHECK(filter.position_update_accepted_count == 1u &&
filter.position_update_rejected_count == 1u,
"GPS accept/reject counters");
}
static double profile_acceleration(double time_s)
{
if((time_s >= 2.0) && (time_s < 4.0))
{
return 2.0;
}
if((time_s >= 6.0) && (time_s < 8.0))
{
return -2.0;
}
return 0.0;
}
static void test_motion_transitions(void)
{
eskf_t filter;
eskf_config_t eskf_config;
identity_filter(&filter, &eskf_config);
fusion_motion_config_t motion_config;
fusion_motion_detector_t detector;
fusion_motion_default_config(&motion_config);
fusion_motion_init(&detector, true);
const double dt_s = 0.01;
double first_moving_time = NAN;
double return_stationary_time = NAN;
bool stationary_during_constant_velocity = false;
bool deceleration_seen = false;
for(unsigned int sample = 1u ; sample <= 1100u ; sample++)
{
double time_s = (double)sample * dt_s;
double acceleration = profile_acceleration(time_s - 0.5 * dt_s);
double accel[3] = {acceleration, 0.0, eskf_config.gravity_mps2};
double gyro[3] = {0.0, 0.0, 0.0};
CHECK(eskf_propagate(&filter, &eskf_config, accel, gyro, dt_s),
"motion profile propagation");
fusion_motion_state_t state = fusion_motion_update(
&detector, &motion_config,
filter.last_linear_accel_nav_mps2,
filter.last_unbiased_gyro_rps,
eskf_speed_mps(&filter),
FUSION_GNSS_MOTION_UNKNOWN,
dt_s);
if(isnan(first_moving_time) && (state == FUSION_MOTION_MOVING))
{
first_moving_time = time_s;
}
if((time_s >= 4.0) && (time_s < 6.0) &&
(state == FUSION_MOTION_STATIONARY))
{
stationary_during_constant_velocity = true;
}
if((time_s >= 6.0) && (time_s < 8.0) &&
(detector.last_linear_accel_norm_mps2 > 1.5))
{
deceleration_seen = true;
}
if((time_s > 8.0) && isnan(return_stationary_time) &&
(state == FUSION_MOTION_STATIONARY))
{
return_stationary_time = time_s;
}
if(fusion_motion_allows_zero_update(&detector) &&
(fmod(time_s, 0.05) < 0.5 * dt_s))
{
CHECK(eskf_zero_velocity_update(&filter, 0.03),
"stationary ZUPT succeeds");
}
}
CHECK(isfinite(first_moving_time) && (first_moving_time <= 2.02),
"sudden acceleration exits stationary within one IMU interval");
CHECK(!stationary_during_constant_velocity,
"constant velocity is not mistaken for stationary");
CHECK(deceleration_seen, "sudden deceleration appears as dynamic evidence");
CHECK(isfinite(return_stationary_time) && (return_stationary_time >= 9.0) &&
(return_stationary_time <= 9.25),
"stationary re-entry waits for jerk settling and quiet dwell");
CHECK(fabs(filter.velocity_mps[0]) < 0.03,
"accelerate/decelerate profile returns close to zero speed");
}
static double run_rate_profile(unsigned int imu_hz, double gps_period_s)
{
eskf_t filter;
eskf_config_t config;
identity_filter(&filter, &config);
double dt_s = 1.0 / (double)imu_hz;
double next_gps_s = gps_period_s;
double truth_position = 0.0;
double truth_velocity = 0.0;
double gps_covariance[9] = {
0.25, 0.0, 0.0,
0.0, 0.25, 0.0,
0.0, 0.0, 1.0
};
for(unsigned int sample = 0 ; sample < 6u * imu_hz ; sample++)
{
double time_s = (double)sample * dt_s;
double acceleration = 0.0;
if((time_s >= 1.0) && (time_s < 2.0))
{
acceleration = 1.0;
}
else if((time_s >= 3.0) && (time_s < 4.0))
{
acceleration = -1.0;
}
truth_position += truth_velocity * dt_s + 0.5 * acceleration * dt_s * dt_s;
truth_velocity += acceleration * dt_s;
double accel[3] = {acceleration, 0.0, config.gravity_mps2};
double gyro[3] = {0.0, 0.0, 0.0};
CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s),
"variable-rate propagation");
double end_time_s = time_s + dt_s;
if((end_time_s + 1.0e-9) >= next_gps_s)
{
double gps_position[3] = {truth_position, 0.0, 0.0};
CHECK(eskf_position_update(&filter, &config, gps_position,
gps_covariance, NULL) == ESKF_UPDATE_ACCEPTED,
"variable-rate asynchronous GPS update");
next_gps_s += gps_period_s;
}
}
CHECK(fabs(truth_position - 2.0) < 1.0e-9,
"synthetic rate profile truth distance");
return filter.position_m[0];
}
static void test_variable_rates(void)
{
double position_100hz = run_rate_profile(100u, 1.0);
double position_200hz = run_rate_profile(200u, 0.5);
double position_500hz = run_rate_profile(500u, 1.0);
CHECK(fabs(position_100hz - 2.0) < 0.02,
"100 Hz IMU / 1 Hz GPS result");
CHECK(fabs(position_200hz - 2.0) < 0.02,
"200 Hz IMU / 2 Hz GPS result");
CHECK(fabs(position_500hz - 2.0) < 0.02,
"500 Hz IMU / 1 Hz GPS result");
CHECK(fabs(position_100hz - position_200hz) < 0.02 &&
fabs(position_200hz - position_500hz) < 0.02,
"filter result is insensitive to configured asynchronous rates");
}
static void test_irregular_imu_timestamps(void)
{
eskf_t filter;
eskf_config_t config;
identity_filter(&filter, &config);
const double intervals_s[] = {0.003, 0.011, 0.004, 0.017, 0.006, 0.009};
double elapsed_s = 0.0;
double truth_position[3] = {0.0, 0.0, 0.0};
double truth_velocity[3] = {0.0, 0.0, 0.0};
const double nav_acceleration[3] = {0.20, -0.10, 0.0};
unsigned int interval_index = 0u;
while(elapsed_s < 5.0)
{
double dt_s = intervals_s[interval_index %
(sizeof(intervals_s) / sizeof(intervals_s[0]))];
if((elapsed_s + dt_s) > 5.0)
{
dt_s = 5.0 - elapsed_s;
}
double accel[3] = {
nav_acceleration[0], nav_acceleration[1], config.gravity_mps2
};
double gyro[3] = {0.0, 0.0, 0.0};
CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s),
"irregular timestamp propagation succeeds");
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
truth_position[axis] += truth_velocity[axis] * dt_s +
0.5 * nav_acceleration[axis] * dt_s * dt_s;
truth_velocity[axis] += nav_acceleration[axis] * dt_s;
}
elapsed_s += dt_s;
interval_index++;
}
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
CHECK(fabs(filter.position_m[axis] - truth_position[axis]) < 1.0e-10,
"irregular local-tick dt preserves position integration");
CHECK(fabs(filter.velocity_mps[axis] - truth_velocity[axis]) < 1.0e-10,
"irregular local-tick dt preserves velocity integration");
}
}
static void test_motion_vetoes(void)
{
fusion_motion_config_t config;
fusion_motion_detector_t detector;
fusion_motion_default_config(&config);
fusion_motion_init(&detector, false);
double zero[3] = {0.0, 0.0, 0.0};
for(unsigned int i = 0 ; i < 200u ; i++)
{
(void)fusion_motion_update(&detector, &config, zero, zero, 3.0,
FUSION_GNSS_MOTION_UNKNOWN, 0.01);
}
CHECK(detector.state == FUSION_MOTION_MOVING,
"nonzero ESKF speed vetoes stationary at constant velocity");
for(unsigned int i = 0 ; i < 200u ; i++)
{
(void)fusion_motion_update(&detector, &config, zero, zero, 3.0,
FUSION_GNSS_MOTION_STILL, 0.01);
}
CHECK(detector.state == FUSION_MOTION_MOVING,
"position-only GNSS still evidence cannot override high ESKF speed");
for(unsigned int i = 0 ; i < 200u ; i++)
{
(void)fusion_motion_update(&detector, &config, zero, zero, 0.0,
FUSION_GNSS_MOTION_MOVING, 0.01);
}
CHECK(detector.state == FUSION_MOTION_MOVING,
"GNSS motion evidence vetoes stationary");
for(unsigned int i = 0 ; i < 120u ; i++)
{
(void)fusion_motion_update(&detector, &config, zero, zero, 0.0,
FUSION_GNSS_MOTION_STILL, 0.01);
}
CHECK(detector.state == FUSION_MOTION_STATIONARY,
"quiet low-speed state enters stationary after dwell");
fusion_motion_init(&detector, true);
double weak_accel[3] = {0.25, 0.0, 0.0};
for(unsigned int i = 0 ; i < config.weak_motion_samples - 1u ; i++)
{
(void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0,
FUSION_GNSS_MOTION_UNKNOWN, 0.01);
}
CHECK(detector.state == FUSION_MOTION_STATIONARY,
"short weak disturbance does not chatter state");
(void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0,
FUSION_GNSS_MOTION_UNKNOWN, 0.01);
CHECK(detector.state == FUSION_MOTION_MOVING,
"persistent weak motion exits stationary");
}
static void test_stationary_noise_and_impulse_detection(void)
{
fusion_motion_config_t config;
fusion_motion_detector_t detector;
fusion_motion_default_config(&config);
fusion_motion_init(&detector, true);
noise_state = 0x12345678u;
const double dt_s = 1.0 / 500.0;
unsigned int nonstationary_samples = 0u;
for(unsigned int sample = 0u ; sample < 5000u ; sample++)
{
double accel[3];
double gyro[3];
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
accel[axis] = 0.025 * gaussian_noise();
gyro[axis] = 0.0008 * gaussian_noise();
}
fusion_motion_state_t state = fusion_motion_update(
&detector, &config, accel, gyro, 0.0,
FUSION_GNSS_MOTION_STILL, dt_s);
if(state != FUSION_MOTION_STATIONARY)
{
nonstationary_samples++;
}
}
CHECK(nonstationary_samples == 0u,
"500 Hz stationary IMU noise does not cause false motion");
double sudden_accel[3] = {1.0, 0.0, 0.0};
double zero_gyro[3] = {0.0, 0.0, 0.0};
CHECK(fusion_motion_update(&detector, &config, sudden_accel, zero_gyro,
0.0, FUSION_GNSS_MOTION_STILL, dt_s) ==
FUSION_MOTION_MOVING,
"filtered jerk still detects a sudden acceleration in one 500 Hz sample");
}
static void initialize_engine_for_replay(fusion_engine_t* engine)
{
fusion_engine_config_t config;
fusion_engine_default_config(&config, 1000u);
config.calibration_duration_s = 0.10;
config.calibration_min_samples = 10u;
CHECK(fusion_engine_init(engine, &config), "fusion engine initialization");
fusion_position_measurement_t origin = {0};
origin.timestamp_ticks = 0u;
origin.source_valid = true;
origin.source_quality = 1.0;
origin.covariance_enu_m2[0] = 0.25;
origin.covariance_enu_m2[4] = 0.25;
origin.covariance_enu_m2[8] = 1.0;
CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL),
"initial position retained during calibration");
for(uint64_t tick = 0u ; tick <= 100u ; tick += 10u)
{
fusion_imu_measurement_t imu = {0};
imu.timestamp_ticks = tick;
imu.accel_mps2[2] = config.eskf.gravity_mps2;
imu.temperature_c = 25.0;
imu.sequence = (uint32_t)(tick / 10u);
imu.vehicle_at_rest = true;
CHECK(fusion_engine_push_imu(engine, &imu),
"stationary calibration IMU accepted");
}
CHECK(engine->state == FUSION_ENGINE_RUNNING,
"fusion engine leaves calibration after quiet duration");
}
static double replay_profile_accel(uint64_t tick)
{
double time_s = (double)tick / 1000.0;
if((time_s > 1.0) && (time_s <= 2.0))
{
return 1.0;
}
if((time_s > 2.0) && (time_s <= 3.0))
{
return -1.0;
}
return 0.0;
}
static void push_replay_imu(fusion_engine_t* engine, uint64_t tick)
{
fusion_imu_measurement_t imu = {0};
imu.timestamp_ticks = tick;
imu.accel_mps2[0] = replay_profile_accel(tick);
imu.accel_mps2[2] = engine->config.eskf.gravity_mps2;
imu.temperature_c = 25.0;
imu.sequence = (uint32_t)(tick / 10u);
imu.vehicle_at_rest = fabs(imu.accel_mps2[0]) < 0.1;
CHECK(fusion_engine_push_imu(engine, &imu), "replay profile IMU accepted");
}
static fusion_position_measurement_t replay_position(uint64_t tick, double east_m)
{
fusion_position_measurement_t position = {0};
position.timestamp_ticks = tick;
position.position_enu_m[0] = east_m;
position.covariance_enu_m2[0] = 0.25;
position.covariance_enu_m2[4] = 0.25;
position.covariance_enu_m2[8] = 1.0;
position.source_quality = 1.0;
position.source_valid = true;
return position;
}
static void check_runtime_close(const fusion_runtime_t* first,
const fusion_runtime_t* second,
double tolerance)
{
for(unsigned int i = 0 ; i < 3u ; i++)
{
CHECK(fabs(first->filter.position_m[i] - second->filter.position_m[i]) < tolerance,
"delayed replay position matches on-time update");
CHECK(fabs(first->filter.velocity_mps[i] - second->filter.velocity_mps[i]) < tolerance,
"delayed replay velocity matches on-time update");
CHECK(fabs(first->filter.accel_bias_mps2[i] - second->filter.accel_bias_mps2[i]) < tolerance,
"delayed replay accel bias matches on-time update");
CHECK(fabs(first->filter.gyro_bias_rps[i] - second->filter.gyro_bias_rps[i]) < tolerance,
"delayed replay gyro bias matches on-time update");
}
for(unsigned int i = 0 ; i < 4u ; i++)
{
CHECK(fabs(first->filter.body_to_nav_quaternion[i] -
second->filter.body_to_nav_quaternion[i]) < tolerance,
"delayed replay attitude matches on-time update");
}
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
CHECK(fabs(first->filter.covariance[row][col] -
second->filter.covariance[row][col]) < tolerance,
"delayed replay covariance matches on-time update");
}
}
CHECK(first->motion.state == second->motion.state,
"delayed replay motion state matches on-time update");
CHECK(first->filter_timestamp_ticks == second->filter_timestamp_ticks,
"delayed replay timestamp matches on-time update");
}
static void test_delayed_position_replay(void)
{
static fusion_engine_t on_time;
static fusion_engine_t delayed;
initialize_engine_for_replay(&on_time);
initialize_engine_for_replay(&delayed);
fusion_position_measurement_t gps_at_1500 = replay_position(1500u, 0.125);
for(uint64_t tick = 110u ; tick <= 3200u ; tick += 10u)
{
push_replay_imu(&on_time, tick);
push_replay_imu(&delayed, tick);
if(tick == 1500u)
{
eskf_update_result_t result = ESKF_UPDATE_INVALID;
CHECK(fusion_engine_push_position(&on_time, &gps_at_1500,
&result, NULL),
"on-time position update processed");
CHECK(result == ESKF_UPDATE_ACCEPTED,
"on-time position update accepted");
}
if(tick == 1800u)
{
eskf_update_result_t result = ESKF_UPDATE_INVALID;
CHECK(fusion_engine_push_position(&delayed, &gps_at_1500,
&result, NULL),
"300 ms delayed position update replayed");
CHECK(result == ESKF_UPDATE_ACCEPTED,
"delayed position update accepted at measurement epoch");
check_runtime_close(&on_time.current, &delayed.current, 1.0e-11);
}
}
check_runtime_close(&on_time.current, &delayed.current, 1.0e-11);
CHECK(delayed.diagnostics.replay_count == 1u,
"one delayed position caused one fixed-lag replay");
CHECK(delayed.diagnostics.replayed_imu_count > 0u,
"delayed position replayed stored IMU samples");
fusion_position_measurement_t future = replay_position(3500u, 1.0);
CHECK(fusion_engine_push_position(&delayed, &future, NULL, NULL),
"future-dated position queued without changing filter time");
CHECK(delayed.pending_position_count == 1u,
"future position is pending until IMU reaches its timestamp");
for(uint64_t tick = 3210u ; tick <= 3500u ; tick += 10u)
{
push_replay_imu(&delayed, tick);
}
CHECK(delayed.pending_position_count == 0u,
"pending position consumed at matching IMU time");
CHECK(delayed.diagnostics.position_accepted == 2u,
"on-time and future position updates accepted");
fusion_position_measurement_t stale = replay_position(1400u, 0.1);
CHECK(!fusion_engine_push_position(&delayed, &stale, NULL, NULL),
"position older than last checkpoint rejected");
CHECK(delayed.diagnostics.position_stale_rejected == 1u,
"stale position rejection is diagnosed");
}
static fusion_gps_measurement_t adapter_gps(double latitude_deg,
double longitude_deg,
double height_m)
{
fusion_gps_measurement_t gps = {0};
gps.latitude_deg = latitude_deg;
gps.longitude_deg = longitude_deg;
gps.height_m = height_m;
gps.covariance_enu_m2[0][0] = 0.04;
gps.covariance_enu_m2[1][1] = 0.09;
gps.covariance_enu_m2[2][2] = 0.25;
gps.chi_square_ratio = 0.5;
gps.solution_valid = true;
return gps;
}
static void test_gps_adapter(void)
{
fusion_gps_adapter_config_t config;
fusion_gps_adapter_t adapter;
fusion_gps_adapter_default_config(&config);
CHECK(fusion_gps_adapter_init(&adapter, &config), "GPS adapter initialization");
fusion_gps_measurement_t origin = adapter_gps(41.015137, 28.979530, 100.0);
fusion_position_measurement_t local;
CHECK(fusion_gps_adapter_convert(&adapter, &origin, &local),
"GPS origin converts to local frame");
CHECK(test_norm3(local.position_enu_m) < 1.0e-8,
"first valid GPS establishes zero ENU origin");
CHECK(nearly_equal(local.covariance_enu_m2[0], 0.25, 1.0e-12) &&
nearly_equal(local.covariance_enu_m2[4], 0.25, 1.0e-12) &&
nearly_equal(local.covariance_enu_m2[8], 1.0, 1.0e-12),
"GPS covariance floors prevent overconfident ESKF updates");
fusion_gps_measurement_t moved = adapter_gps(41.015147, 28.979550, 103.0);
moved.timestamp_ticks = 1000u;
CHECK(fusion_gps_adapter_convert(&adapter, &moved, &local),
"second GPS converts to ENU");
CHECK(local.position_enu_m[0] > 1.0 && local.position_enu_m[1] > 0.5,
"latitude/longitude change has expected ENU direction");
double latitude;
double longitude;
double height;
CHECK(fusion_gps_adapter_local_to_geodetic(&adapter, local.position_enu_m,
&latitude, &longitude, &height),
"fused ENU converts back to geodetic telemetry");
CHECK(fabs(latitude - moved.latitude_deg) < 1.0e-8 &&
fabs(longitude - moved.longitude_deg) < 1.0e-8 &&
fabs(height - moved.height_m) < 1.0e-3,
"ENU/geodetic round trip is accurate");
fusion_gps_measurement_t invalid = moved;
invalid.covariance_enu_m2[0][0] = -1.0;
CHECK(!fusion_gps_adapter_convert(&adapter, &invalid, &local),
"invalid GPS covariance is rejected before ESKF");
}
static void test_measurement_bus(void)
{
static fusion_bus_t bus;
fusion_bus_init(&bus);
for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++)
{
fusion_imu_measurement_t imu = {0};
imu.sequence = sequence;
imu.timestamp_ticks = sequence;
CHECK(fusion_bus_publish_imu(&bus, &imu), "IMU bus enqueue");
}
fusion_imu_measurement_t overflow = {0};
CHECK(!fusion_bus_publish_imu(&bus, &overflow),
"full IMU bus rejects newest sample without overwriting history");
CHECK(bus.imu_overflow_count == 1u, "IMU bus overflow counter");
for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++)
{
fusion_imu_measurement_t imu;
CHECK(fusion_bus_take_imu(&bus, &imu), "IMU bus dequeue");
CHECK(imu.sequence == sequence, "IMU bus preserves FIFO ordering");
}
CHECK(!fusion_bus_take_imu(&bus, &overflow), "empty IMU bus reports no sample");
for(unsigned int sequence = 0u ; sequence < FUSION_GPS_QUEUE_CAPACITY - 1u ; sequence++)
{
fusion_gps_measurement_t gps = {0};
gps.timestamp_ticks = sequence;
CHECK(fusion_bus_publish_gps(&bus, &gps), "GPS bus enqueue");
}
fusion_gps_measurement_t gps_overflow = {0};
CHECK(!fusion_bus_publish_gps(&bus, &gps_overflow), "GPS bus overflow detected");
CHECK(bus.gps_overflow_count == 1u, "GPS bus overflow counter");
}
static void test_history_rolls_checkpoint_forward(void)
{
static fusion_engine_t engine;
initialize_engine_for_replay(&engine);
uint64_t tick = 110u;
for(unsigned int i = 0u ; i < FUSION_IMU_HISTORY_CAPACITY + 64u ; i++, tick += 10u)
{
push_replay_imu(&engine, tick);
}
CHECK(engine.state == FUSION_ENGINE_RUNNING,
"long GPS outage does not fault fusion engine");
CHECK(engine.diagnostics.imu_history_overflow == 64u,
"fixed-lag history reports bounded rollover");
CHECK(engine.diagnostics.checkpoint_advance_count == 64u,
"checkpoint advances with each expired IMU sample");
CHECK(engine.checkpoint.filter_timestamp_ticks > 100u,
"checkpoint no longer precedes retained history");
fusion_position_measurement_t recovery = replay_position(tick - 10u, 0.0);
eskf_update_result_t result = ESKF_UPDATE_INVALID;
CHECK(fusion_engine_push_position(&engine, &recovery, &result, NULL),
"GPS recovery after history rollover is processed");
CHECK(result == ESKF_UPDATE_ACCEPTED,
"GPS recovery after long outage passes consistent NIS");
}
int main(void)
{
test_stationary_propagation();
test_zero_update_primitives();
test_external_rest_cannot_erase_constant_turn();
test_gravity_alignment();
test_position_nis_gate();
test_motion_transitions();
test_variable_rates();
test_irregular_imu_timestamps();
test_motion_vetoes();
test_stationary_noise_and_impulse_detection();
test_delayed_position_replay();
test_gps_adapter();
test_measurement_bus();
test_history_rolls_checkpoint_forward();
if(failures != 0)
{
fprintf(stderr, "%d ESKF/motion test(s) failed\n", failures);
return 1;
}
printf("ESKF/motion tests passed (gravity, NIS, replay, noise, 100/200/500 Hz).\n");
return 0;
}
Reference in New Issue View Git Blame Copy Permalink