#include #include #include #include #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; }