#define _POSIX_C_SOURCE 200809L #include #include #include #include #include #include #include #include #include "gps/gps_nav_epoch.h" #include "gps/gps_time_anchor.h" #define MAX_SATELLITES 9 #define STATE_DIM 4 #define PI 3.14159265358979323846 #define RECEIVER_SAMPLES_PER_SECOND 16368000.0 #define EPOCH_SAMPLE_WINDOW 409200u #define EPOCH_TOW_TOLERANCE_SEC 0.001 typedef struct { unsigned prn; double position_ecef_m[3]; double pseudorange_m; double sigma_m; } measurement_t; typedef struct { double state[STATE_DIM]; double covariance_enu_m2[3][3]; double hacc_m; double vacc_m; double residual_rms_m; double weighted_residual_rms_m; double chi_square; double chi_square_threshold; double chi_square_ratio; double quality_score; unsigned used_count; unsigned dof; uint32_t used_mask; uint32_t excluded_mask; bool residual_test_valid; bool chi_square_pass; bool fde_applied; bool solution_valid; } solution_t; typedef enum { SOLVE_ABSOLUTE_WEIGHTS, SOLVE_NORMALIZED_WEIGHTS, SOLVE_UNIFORM_WEIGHTS } solve_weight_mode_t; static int failures; static volatile double benchmark_sink; #define CHECK(condition, message) do { \ if (!(condition)) { \ fprintf(stderr, "FAIL: %s\n", message); \ failures++; \ } \ } while (0) static double square(double x) { return x * x; } static double vector_norm3(const double v[3]) { return sqrt(square(v[0]) + square(v[1]) + square(v[2])); } static double position_error_m(const solution_t* solution, const double truth[3]) { double error[3] = { solution->state[0] - truth[0], solution->state[1] - truth[1], solution->state[2] - truth[2] }; return vector_norm3(error); } static bool inverse4(double input[STATE_DIM][STATE_DIM], double inverse[STATE_DIM][STATE_DIM]) { double augmented[STATE_DIM][2 * STATE_DIM]; double scale = 0.0; for (unsigned row = 0; row < STATE_DIM; row++) { for (unsigned col = 0; col < STATE_DIM; col++) { augmented[row][col] = input[row][col]; augmented[row][col + STATE_DIM] = (row == col) ? 1.0 : 0.0; if (fabs(input[row][col]) > scale) { scale = fabs(input[row][col]); } } } if (!(scale > 0.0) || !isfinite(scale)) { return false; } for (unsigned pivot = 0; pivot < STATE_DIM; pivot++) { unsigned pivot_row = pivot; double pivot_magnitude = fabs(augmented[pivot][pivot]); for (unsigned row = pivot + 1; row < STATE_DIM; row++) { double magnitude = fabs(augmented[row][pivot]); if (magnitude > pivot_magnitude) { pivot_magnitude = magnitude; pivot_row = row; } } if (pivot_magnitude <= scale * DBL_EPSILON * 128.0) { return false; } if (pivot_row != pivot) { for (unsigned col = 0; col < 2 * STATE_DIM; col++) { double temporary = augmented[pivot][col]; augmented[pivot][col] = augmented[pivot_row][col]; augmented[pivot_row][col] = temporary; } } double divisor = augmented[pivot][pivot]; for (unsigned col = 0; col < 2 * STATE_DIM; col++) { augmented[pivot][col] /= divisor; } for (unsigned row = 0; row < STATE_DIM; row++) { if (row == pivot) { continue; } double factor = augmented[row][pivot]; for (unsigned col = 0; col < 2 * STATE_DIM; col++) { augmented[row][col] -= factor * augmented[pivot][col]; } } } for (unsigned row = 0; row < STATE_DIM; row++) { for (unsigned col = 0; col < STATE_DIM; col++) { inverse[row][col] = augmented[row][col + STATE_DIM]; if (!isfinite(inverse[row][col])) { return false; } } } return true; } static bool solve_normal_equations(double normal[STATE_DIM][STATE_DIM], const double rhs[STATE_DIM], double update[STATE_DIM]) { double inverse[STATE_DIM][STATE_DIM]; if (!inverse4(normal, inverse)) { return false; } for (unsigned row = 0; row < STATE_DIM; row++) { update[row] = 0.0; for (unsigned col = 0; col < STATE_DIM; col++) { update[row] += inverse[row][col] * rhs[col]; } } return true; } static double chi_square_threshold_99(unsigned dof) { static const double thresholds[] = { 0.0, 6.634896601, 9.210340372, 11.34486673, 13.27670414, 15.08627247 }; return (dof < (sizeof(thresholds) / sizeof(thresholds[0]))) ? thresholds[dof] : 0.0; } static void geodetic_to_ecef(double latitude_rad, double longitude_rad, double height_m, double ecef_m[3]) { const double semi_major_axis_m = 6378137.0; const double eccentricity_squared = 6.6943799901413165e-3; double sin_latitude = sin(latitude_rad); double cos_latitude = cos(latitude_rad); double prime_vertical_radius = semi_major_axis_m / sqrt(1.0 - eccentricity_squared * square(sin_latitude)); ecef_m[0] = (prime_vertical_radius + height_m) * cos_latitude * cos(longitude_rad); ecef_m[1] = (prime_vertical_radius + height_m) * cos_latitude * sin(longitude_rad); ecef_m[2] = (prime_vertical_radius * (1.0 - eccentricity_squared) + height_m) * sin_latitude; } static void enu_axes(double latitude_rad, double longitude_rad, double axes[3][3]) { double sin_latitude = sin(latitude_rad); double cos_latitude = cos(latitude_rad); double sin_longitude = sin(longitude_rad); double cos_longitude = cos(longitude_rad); axes[0][0] = -sin_longitude; axes[0][1] = cos_longitude; axes[0][2] = 0.0; axes[1][0] = -sin_latitude * cos_longitude; axes[1][1] = -sin_latitude * sin_longitude; axes[1][2] = cos_latitude; axes[2][0] = cos_latitude * cos_longitude; axes[2][1] = cos_latitude * sin_longitude; axes[2][2] = sin_latitude; } static void create_measurements(measurement_t measurements[MAX_SATELLITES], unsigned count, const double receiver_ecef_m[3], double latitude_rad, double longitude_rad, double receiver_clock_bias_m, const double noise_m[MAX_SATELLITES]) { static const double azimuth_deg[MAX_SATELLITES] = {5.0, 52.0, 104.0, 158.0, 214.0, 267.0, 318.0, 74.0, 242.0}; static const double elevation_deg[MAX_SATELLITES] = {72.0, 46.0, 31.0, 57.0, 26.0, 41.0, 12.0, 20.0, 35.0}; double axes[3][3]; enu_axes(latitude_rad, longitude_rad, axes); for (unsigned index = 0; index < count; index++) { double azimuth = azimuth_deg[index] * PI / 180.0; double elevation = elevation_deg[index] * PI / 180.0; double los_enu[3] = { cos(elevation) * sin(azimuth), cos(elevation) * cos(azimuth), sin(elevation) }; double los_ecef[3] = {0.0, 0.0, 0.0}; for (unsigned component = 0; component < 3; component++) { for (unsigned enu = 0; enu < 3; enu++) { los_ecef[component] += axes[enu][component] * los_enu[enu]; } } double geometric_range_m = 20200000.0 + 170000.0 * index; measurements[index].prn = index + 1; for (unsigned component = 0; component < 3; component++) { measurements[index].position_ecef_m[component] = receiver_ecef_m[component] + geometric_range_m * los_ecef[component]; } double elevation_sigma_m = 3.0 / sin(elevation); measurements[index].sigma_m = sqrt(square(2.4) + square(elevation_sigma_m)); measurements[index].pseudorange_m = geometric_range_m + receiver_clock_bias_m + noise_m[index]; } } static bool solve_subset(const measurement_t measurements[MAX_SATELLITES], unsigned available_count, uint32_t used_mask, solve_weight_mode_t weight_mode, const double seed[STATE_DIM], double latitude_rad, double longitude_rad, solution_t* output) { memset(output, 0, sizeof(*output)); output->used_mask = used_mask; output->excluded_mask = ((1u << available_count) - 1u) & ~used_mask; for (unsigned index = 0; index < available_count; index++) { if ((used_mask & (1u << index)) != 0u) { output->used_count++; } } if (output->used_count < STATE_DIM) { return false; } memcpy(output->state, seed, sizeof(output->state)); for (unsigned iteration = 0; iteration < 20; iteration++) { double normal[STATE_DIM][STATE_DIM] = {{0.0}}; double rhs[STATE_DIM] = {0.0}; double maximum_absolute_weight = 0.0; for (unsigned index = 0; index < available_count; index++) { if ((used_mask & (1u << index)) == 0u) { continue; } double absolute_weight = 1.0 / square(measurements[index].sigma_m); if (absolute_weight > maximum_absolute_weight) { maximum_absolute_weight = absolute_weight; } } for (unsigned index = 0; index < available_count; index++) { if ((used_mask & (1u << index)) == 0u) { continue; } double receiver_to_satellite[3]; for (unsigned component = 0; component < 3; component++) { receiver_to_satellite[component] = output->state[component] - measurements[index].position_ecef_m[component]; } double geometric_range_m = vector_norm3(receiver_to_satellite); if (!(geometric_range_m > 1.0)) { return false; } double design_row[STATE_DIM] = { receiver_to_satellite[0] / geometric_range_m, receiver_to_satellite[1] / geometric_range_m, receiver_to_satellite[2] / geometric_range_m, 1.0 }; double residual_m = measurements[index].pseudorange_m - geometric_range_m - output->state[3]; double absolute_weight = 1.0 / square(measurements[index].sigma_m); double solve_weight = absolute_weight; if (weight_mode == SOLVE_NORMALIZED_WEIGHTS) { solve_weight /= maximum_absolute_weight; } else if (weight_mode == SOLVE_UNIFORM_WEIGHTS) { solve_weight = 1.0; } for (unsigned row = 0; row < STATE_DIM; row++) { rhs[row] += solve_weight * design_row[row] * residual_m; for (unsigned col = 0; col < STATE_DIM; col++) { normal[row][col] += solve_weight * design_row[row] * design_row[col]; } } } double update[STATE_DIM]; if (!solve_normal_equations(normal, rhs, update)) { return false; } for (unsigned index = 0; index < STATE_DIM; index++) { output->state[index] += update[index]; } double position_update[3] = {update[0], update[1], update[2]}; if (vector_norm3(position_update) < 1.0e-5 && fabs(update[3]) < 1.0e-5) { break; } if (iteration == 19) { return false; } } double absolute_normal[STATE_DIM][STATE_DIM] = {{0.0}}; double residual_square_sum = 0.0; double absolute_weight_sum = 0.0; output->chi_square = 0.0; for (unsigned index = 0; index < available_count; index++) { if ((used_mask & (1u << index)) == 0u) { continue; } double receiver_to_satellite[3]; for (unsigned component = 0; component < 3; component++) { receiver_to_satellite[component] = output->state[component] - measurements[index].position_ecef_m[component]; } double geometric_range_m = vector_norm3(receiver_to_satellite); double design_row[STATE_DIM] = { receiver_to_satellite[0] / geometric_range_m, receiver_to_satellite[1] / geometric_range_m, receiver_to_satellite[2] / geometric_range_m, 1.0 }; double residual_m = measurements[index].pseudorange_m - geometric_range_m - output->state[3]; double absolute_weight = 1.0 / square(measurements[index].sigma_m); residual_square_sum += square(residual_m); output->chi_square += absolute_weight * square(residual_m); absolute_weight_sum += absolute_weight; for (unsigned row = 0; row < STATE_DIM; row++) { for (unsigned col = 0; col < STATE_DIM; col++) { absolute_normal[row][col] += absolute_weight * design_row[row] * design_row[col]; } } } output->residual_rms_m = sqrt(residual_square_sum / output->used_count); output->weighted_residual_rms_m = sqrt(output->chi_square / absolute_weight_sum); output->dof = output->used_count - STATE_DIM; output->chi_square_threshold = chi_square_threshold_99(output->dof); output->residual_test_valid = output->dof > 0 && output->chi_square_threshold > 0.0; if (output->residual_test_valid) { output->chi_square_ratio = output->chi_square / output->chi_square_threshold; output->quality_score = 100.0 / (1.0 + output->chi_square_ratio); output->chi_square_pass = output->chi_square <= output->chi_square_threshold; } else { output->chi_square_ratio = INFINITY; output->quality_score = 0.0; } output->solution_valid = ((output->used_count == STATE_DIM) && (output->dof == 0u)) || (output->residual_test_valid && output->chi_square_pass); double covariance_ecef[STATE_DIM][STATE_DIM]; if (!inverse4(absolute_normal, covariance_ecef)) { return false; } double axes[3][3]; enu_axes(latitude_rad, longitude_rad, axes); for (unsigned enu_row = 0; enu_row < 3; enu_row++) { for (unsigned enu_col = 0; enu_col < 3; enu_col++) { double covariance = 0.0; for (unsigned ecef_row = 0; ecef_row < 3; ecef_row++) { for (unsigned ecef_col = 0; ecef_col < 3; ecef_col++) { covariance += axes[enu_row][ecef_row] * covariance_ecef[ecef_row][ecef_col] * axes[enu_col][ecef_col]; } } output->covariance_enu_m2[enu_row][enu_col] = covariance; } } output->hacc_m = sqrt(output->covariance_enu_m2[0][0] + output->covariance_enu_m2[1][1]); output->vacc_m = sqrt(output->covariance_enu_m2[2][2]); return isfinite(output->hacc_m) && isfinite(output->vacc_m); } static bool solve_with_fde_minimum(const measurement_t measurements[MAX_SATELLITES], unsigned count, unsigned minimum_available_for_fde, const double seed[STATE_DIM], double latitude_rad, double longitude_rad, solution_t* selected, solution_t* full_solution) { uint32_t full_mask = (1u << count) - 1u; if (!solve_subset(measurements, count, full_mask, SOLVE_NORMALIZED_WEIGHTS, seed, latitude_rad, longitude_rad, full_solution)) { return false; } *selected = *full_solution; if (full_solution->chi_square_pass || count < minimum_available_for_fde) { return true; } bool passing_candidate_found = false; for (unsigned excluded = 0; excluded < count; excluded++) { solution_t candidate; uint32_t candidate_mask = full_mask & ~(1u << excluded); if (!solve_subset(measurements, count, candidate_mask, SOLVE_NORMALIZED_WEIGHTS, full_solution->state, latitude_rad, longitude_rad, &candidate)) { continue; } bool choose = false; if (candidate.chi_square_pass) { choose = !passing_candidate_found || candidate.chi_square_ratio < selected->chi_square_ratio; } else if (!passing_candidate_found && candidate.chi_square_ratio < selected->chi_square_ratio) { choose = true; } if (choose) { *selected = candidate; selected->fde_applied = true; } if (candidate.chi_square_pass) { passing_candidate_found = true; } } return true; } static bool solve_with_fde(const measurement_t measurements[MAX_SATELLITES], unsigned count, const double seed[STATE_DIM], double latitude_rad, double longitude_rad, solution_t* selected, solution_t* full_solution) { return solve_with_fde_minimum(measurements, count, 7u, seed, latitude_rad, longitude_rad, selected, full_solution); } static void print_solution(const char* label, const solution_t* solution, const double receiver_truth[3]) { printf("%-24s used=%u excluded=0x%03x error=%7.2f m residual=%7.2f m " "chi2=%8.2f dof=%u ratio=%6.2f quality=%6.1f hAcc=%5.2f vAcc=%5.2f " "pass=%u valid=%u\n", label, solution->used_count, solution->excluded_mask, position_error_m(solution, receiver_truth), solution->residual_rms_m, solution->chi_square, solution->dof, solution->chi_square_ratio, solution->quality_score, solution->hacc_m, solution->vacc_m, solution->chi_square_pass, solution->solution_valid); } static double monotonic_seconds(void) { struct timespec now; clock_gettime(CLOCK_MONOTONIC, &now); return (double)now.tv_sec + (double)now.tv_nsec * 1.0e-9; } static int run_benchmark(const char* scenario, unsigned iterations, const measurement_t clean[MAX_SATELLITES], unsigned satellite_count, const double seed[STATE_DIM], double latitude_rad, double longitude_rad) { measurement_t measurements[MAX_SATELLITES]; memcpy(measurements, clean, sizeof(measurements)); if (strcmp(scenario, "clean") == 0) { /* No injected fault: one WLS plus integrity/covariance work. */ } else if (strcmp(scenario, "single") == 0) { measurements[2].pseudorange_m += 100.0; } else if (strcmp(scenario, "double") == 0) { measurements[2].pseudorange_m += 100.0; measurements[5].pseudorange_m += 50.0; } else { fprintf(stderr, "unknown benchmark scenario: %s\n", scenario); return 2; } solution_t selected; solution_t full; for (unsigned warmup = 0; warmup < 1000; warmup++) { if (!solve_with_fde(measurements, satellite_count, seed, latitude_rad, longitude_rad, &selected, &full)) { return 3; } benchmark_sink += selected.quality_score * 1.0e-12; } double start = monotonic_seconds(); for (unsigned iteration = 0; iteration < iterations; iteration++) { /* Prevent whole-loop constant folding while keeping the scenario's * integrity classification unchanged. */ measurements[0].pseudorange_m += (iteration & 1u) ? 1.0e-3 : -1.0e-3; if (!solve_with_fde(measurements, satellite_count, seed, latitude_rad, longitude_rad, &selected, &full)) { return 3; } benchmark_sink += selected.quality_score * 1.0e-12; } double elapsed = monotonic_seconds() - start; double nanoseconds_per_solution = elapsed * 1.0e9 / (double)iterations; printf("benchmark scenario=%s iterations=%u elapsed=%.6f s " "per_nav=%.1f ns (%.3f us) cpu@1Hz=%.6f%% cpu@10Hz=%.6f%% sink=%.9f\n", scenario, iterations, elapsed, nanoseconds_per_solution, nanoseconds_per_solution / 1000.0, nanoseconds_per_solution / 1.0e7, nanoseconds_per_solution / 1.0e6, benchmark_sink); return 0; } typedef struct { uint64_t state; } synthetic_rng_t; typedef struct { bool have_sf[3]; unsigned char iod[3]; bool ephemeris_valid; } linux_nav_channel_t; typedef struct { const char* name; unsigned epochs; unsigned candidate_total; unsigned selected_total; unsigned fixes; unsigned fde_fixes; unsigned epoch_exclusions; double east_sum_m; double north_sum_m; double up_sum_m; double east_square_sum_m2; double north_square_sum_m2; double up_square_sum_m2; double horizontal_max_m; double vertical_max_m; double horizontal_errors_m[128]; unsigned error_count; } synthetic_stage_metrics_t; enum { STAGE_BASELINE = 0, STAGE_ONE_DROPOUT, STAGE_THREE_DROPOUTS, STAGE_MINIMUM_FOUR, STAGE_OUTAGE_THREE, STAGE_REACQUIRE, STAGE_DELAYED_CHANNEL, STAGE_EPHEMERIS_JOIN, STAGE_PSEUDORANGE_FAULT, SYNTHETIC_STAGE_COUNT }; static uint64_t synthetic_rng_next(synthetic_rng_t* rng) { uint64_t value = rng->state; value ^= value >> 12; value ^= value << 25; value ^= value >> 27; rng->state = value; return value * UINT64_C(2685821657736338717); } static double synthetic_rng_uniform(synthetic_rng_t* rng) { return ((double)(synthetic_rng_next(rng) >> 11) + 0.5) / 9007199254740992.0; } static double synthetic_rng_gaussian(synthetic_rng_t* rng) { double u1 = synthetic_rng_uniform(rng); double u2 = synthetic_rng_uniform(rng); return sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2); } static void linux_nav_receive_ephemeris_subframe(linux_nav_channel_t* channel, unsigned subframe_id, unsigned char iod) { if (channel == NULL || subframe_id < 1u || subframe_id > 3u) { return; } unsigned slot = subframe_id - 1u; channel->have_sf[slot] = true; channel->iod[slot] = iod; if (channel->have_sf[0] && channel->have_sf[1] && channel->have_sf[2] && channel->iod[0] == channel->iod[1] && channel->iod[1] == channel->iod[2]) { channel->ephemeris_valid = true; } } static unsigned synthetic_stage_for_epoch(unsigned epoch) { static const unsigned ends[SYNTHETIC_STAGE_COUNT] = {100u, 160u, 220u, 260u, 300u, 360u, 420u, 480u, 540u}; for (unsigned stage = 0u; stage < SYNTHETIC_STAGE_COUNT; stage++) { if (epoch < ends[stage]) { return stage; } } return SYNTHETIC_STAGE_COUNT - 1u; } static uint32_t synthetic_active_mask(unsigned stage) { static const uint32_t masks[SYNTHETIC_STAGE_COUNT] = { 0x0ffu, /* eight healthy channels */ 0x07fu, /* one channel lost */ 0x01fu, /* three channels lost */ 0x00fu, /* minimum 3-D fix plus clock */ 0x007u, /* outage: below the four-channel minimum */ 0x07fu, /* reacquisition */ 0x0ffu, /* one of eight arrives in the wrong epoch */ 0x1ffu, /* ninth channel joins after SF1/SF2/SF3 commit */ 0x1ffu /* one coherent 100 m pseudorange fault */ }; return masks[stage]; } static void synthetic_epoch_time(unsigned epoch, int* week, double* tow_sec) { double absolute_tow = 604790.0 + 0.2 * (double)epoch; *week = 2300 + (int)floor(absolute_tow / 604800.0); *tow_sec = fmod(absolute_tow, 604800.0); } static void synthetic_shift_time(int* week, double* tow_sec, double delta_sec) { *tow_sec += delta_sec; while (*tow_sec >= 604800.0) { *tow_sec -= 604800.0; (*week)++; } while (*tow_sec < 0.0) { *tow_sec += 604800.0; (*week)--; } } static int compare_double(const void* left, const void* right) { double lhs = *(const double*)left; double rhs = *(const double*)right; return (lhs > rhs) - (lhs < rhs); } static void synthetic_record_error(synthetic_stage_metrics_t* metrics, const solution_t* solution, const double receiver_truth[3], double axes[3][3]) { double ecef_error[3] = { solution->state[0] - receiver_truth[0], solution->state[1] - receiver_truth[1], solution->state[2] - receiver_truth[2] }; double enu_error[3] = {0.0, 0.0, 0.0}; for (unsigned enu = 0u; enu < 3u; enu++) { for (unsigned ecef = 0u; ecef < 3u; ecef++) { enu_error[enu] += axes[enu][ecef] * ecef_error[ecef]; } } double horizontal_error = hypot(enu_error[0], enu_error[1]); double vertical_error = fabs(enu_error[2]); metrics->east_sum_m += enu_error[0]; metrics->north_sum_m += enu_error[1]; metrics->up_sum_m += enu_error[2]; metrics->east_square_sum_m2 += square(enu_error[0]); metrics->north_square_sum_m2 += square(enu_error[1]); metrics->up_square_sum_m2 += square(enu_error[2]); if (horizontal_error > metrics->horizontal_max_m) { metrics->horizontal_max_m = horizontal_error; } if (vertical_error > metrics->vertical_max_m) { metrics->vertical_max_m = vertical_error; } if (metrics->error_count < sizeof(metrics->horizontal_errors_m) / sizeof(metrics->horizontal_errors_m[0])) { metrics->horizontal_errors_m[metrics->error_count++] = horizontal_error; } } static void synthetic_print_metrics(synthetic_stage_metrics_t metrics) { double count = (double)metrics.fixes; double horizontal_rmse = count > 0.0 ? sqrt((metrics.east_square_sum_m2 + metrics.north_square_sum_m2) / count) : 0.0; double vertical_rmse = count > 0.0 ? sqrt(metrics.up_square_sum_m2 / count) : 0.0; double horizontal_spread = 0.0; double vertical_spread = 0.0; if (count > 0.0) { double east_mean = metrics.east_sum_m / count; double north_mean = metrics.north_sum_m / count; double up_mean = metrics.up_sum_m / count; double east_variance = metrics.east_square_sum_m2 / count - square(east_mean); double north_variance = metrics.north_square_sum_m2 / count - square(north_mean); double up_variance = metrics.up_square_sum_m2 / count - square(up_mean); horizontal_spread = sqrt(fmax(0.0, east_variance) + fmax(0.0, north_variance)); vertical_spread = sqrt(fmax(0.0, up_variance)); } double p95 = 0.0; if (metrics.error_count != 0u) { qsort(metrics.horizontal_errors_m, metrics.error_count, sizeof(metrics.horizontal_errors_m[0]), compare_double); unsigned p95_index = (95u * metrics.error_count + 99u) / 100u; if (p95_index != 0u) { p95_index--; } p95 = metrics.horizontal_errors_m[p95_index]; } printf("%-21s epochs=%3u fixes=%3u avg=%4.1f->%4.1f " "epoch_excl=%3u fde=%3u hRMSE=%6.2f hSTD=%6.2f " "hP95=%6.2f hMAX=%6.2f vRMSE=%6.2f vSTD=%6.2f\n", metrics.name, metrics.epochs, metrics.fixes, metrics.epochs != 0u ? (double)metrics.candidate_total / (double)metrics.epochs : 0.0, metrics.epochs != 0u ? (double)metrics.selected_total / (double)metrics.epochs : 0.0, metrics.epoch_exclusions, metrics.fde_fixes, horizontal_rmse, horizontal_spread, p95, metrics.horizontal_max_m, vertical_rmse, vertical_spread); } static void run_general_synthetic_scenario(const double receiver_truth[3], double latitude_rad, double longitude_rad, double clock_bias_m) { static const char* names[SYNTHETIC_STAGE_COUNT] = { "baseline-8", "one-dropout-7", "three-dropouts-5", "minimum-fix-4", "outage-3", "reacquire-7", "delayed-channel", "sf123-join", "100m-fault-FDE" }; synthetic_stage_metrics_t metrics[SYNTHETIC_STAGE_COUNT]; memset(metrics, 0, sizeof(metrics)); for (unsigned stage = 0u; stage < SYNTHETIC_STAGE_COUNT; stage++) { metrics[stage].name = names[stage]; } linux_nav_channel_t channels[MAX_SATELLITES]; memset(channels, 0, sizeof(channels)); for (unsigned satellite = 0u; satellite < 8u; satellite++) { channels[satellite].ephemeris_valid = true; } linux_nav_channel_t iod_regression = {0}; linux_nav_receive_ephemeris_subframe(&iod_regression, 1u, 17u); linux_nav_receive_ephemeris_subframe(&iod_regression, 2u, 17u); linux_nav_receive_ephemeris_subframe(&iod_regression, 3u, 18u); CHECK(!iod_regression.ephemeris_valid, "mismatched SF1/SF2/SF3 IOD must not make a channel ready"); linux_nav_receive_ephemeris_subframe(&iod_regression, 3u, 17u); CHECK(iod_regression.ephemeris_valid, "matching SF1/SF2/SF3 must make a channel ready without SF4/SF5"); double axes[3][3]; enu_axes(latitude_rad, longitude_rad, axes); synthetic_rng_t rng = {UINT64_C(0x91e10da5c79e7b1d)}; double seed[STATE_DIM] = { receiver_truth[0] + 120.0, receiver_truth[1] - 90.0, receiver_truth[2] + 60.0, clock_bias_m - 200.0 }; unsigned delayed_selection_failures = 0u; unsigned below_minimum_fixes = 0u; unsigned precommit_join_count_failures = 0u; unsigned postcommit_join_count_failures = 0u; unsigned fde_wrong_exclusion = 0u; bool saw_week_rollover = false; bool saw_sample_rollover = false; uint32_t previous_raw_sample = 0u; for (unsigned epoch = 0u; epoch < 540u; epoch++) { unsigned stage = synthetic_stage_for_epoch(epoch); synthetic_stage_metrics_t* stage_metrics = &metrics[stage]; stage_metrics->epochs++; if (stage == STAGE_EPHEMERIS_JOIN && epoch == 420u) { linux_nav_receive_ephemeris_subframe(&channels[8], 1u, 29u); linux_nav_receive_ephemeris_subframe(&channels[8], 2u, 29u); } if (stage == STAGE_EPHEMERIS_JOIN && epoch == 450u) { linux_nav_receive_ephemeris_subframe(&channels[8], 3u, 29u); } const double zero_noise[MAX_SATELLITES] = {0.0}; measurement_t master[MAX_SATELLITES]; create_measurements(master, MAX_SATELLITES, receiver_truth, latitude_rad, longitude_rad, clock_bias_m, zero_noise); for (unsigned satellite = 0u; satellite < MAX_SATELLITES; satellite++) { master[satellite].pseudorange_m += 0.35 * master[satellite].sigma_m * synthetic_rng_gaussian(&rng); } if (stage == STAGE_PSEUDORANGE_FAULT) { master[2].pseudorange_m += 100.0; } int week; double tow_sec; synthetic_epoch_time(epoch, &week, &tow_sec); if (week > 2300) { saw_week_rollover = true; } uint32_t raw_sample = (UINT32_MAX - 100000u) + epoch * 3273600u; if (epoch != 0u && raw_sample < previous_raw_sample) { saw_sample_rollover = true; } previous_raw_sample = raw_sample; gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES]; unsigned candidate_to_satellite[GPS_NAV_EPOCH_MAX_CANDIDATES]; unsigned candidate_count = 0u; uint32_t active_mask = synthetic_active_mask(stage); for (unsigned satellite = 0u; satellite < MAX_SATELLITES; satellite++) { if ((active_mask & (1u << satellite)) == 0u || !channels[satellite].ephemeris_valid) { continue; } candidates[candidate_count].week = week; candidates[candidate_count].tow_sec = tow_sec; candidates[candidate_count].raw_sample = raw_sample + satellite * 5000u; if (stage == STAGE_DELAYED_CHANNEL && satellite == 7u) { double offset_sec = (epoch & 1u) != 0u ? 0.4 : -0.4; synthetic_shift_time(&candidates[candidate_count].week, &candidates[candidate_count].tow_sec, offset_sec); candidates[candidate_count].raw_sample += (epoch & 1u) != 0u ? 6547200u : (uint32_t)-6547200; } candidate_to_satellite[candidate_count] = satellite; candidate_count++; } stage_metrics->candidate_total += candidate_count; gps_nav_epoch_selection_t selection; bool selected = gps_nav_epoch_select( candidates, candidate_count, EPOCH_SAMPLE_WINDOW, EPOCH_TOW_TOLERANCE_SEC, &selection); unsigned selected_count = selected ? selection.selected_count : 0u; stage_metrics->selected_total += selected_count; stage_metrics->epoch_exclusions += candidate_count - selected_count; if (stage == STAGE_DELAYED_CHANNEL && (selected_count != 7u || (selection.selected_mask & (1u << 7)) != 0u)) { delayed_selection_failures++; } if (stage == STAGE_EPHEMERIS_JOIN && epoch < 450u && selected_count != 8u) { precommit_join_count_failures++; } if (stage == STAGE_EPHEMERIS_JOIN && epoch >= 450u && selected_count != 9u) { postcommit_join_count_failures++; } if (selected_count < STATE_DIM) { continue; } measurement_t selected_measurements[MAX_SATELLITES]; unsigned destination = 0u; for (unsigned candidate = 0u; candidate < candidate_count; candidate++) { if ((selection.selected_mask & (1u << candidate)) != 0u) { selected_measurements[destination++] = master[candidate_to_satellite[candidate]]; } } solution_t solution; solution_t full_solution; if (!solve_with_fde(selected_measurements, selected_count, seed, latitude_rad, longitude_rad, &solution, &full_solution) || !solution.solution_valid) { continue; } stage_metrics->fixes++; if (solution.fde_applied) { stage_metrics->fde_fixes++; } if (stage == STAGE_OUTAGE_THREE) { below_minimum_fixes++; } if (stage == STAGE_PSEUDORANGE_FAULT && (!solution.fde_applied || (solution.excluded_mask & (1u << 2)) == 0u)) { fde_wrong_exclusion++; } synthetic_record_error(stage_metrics, &solution, receiver_truth, axes); memcpy(seed, solution.state, sizeof(seed)); } puts("\nGeneral synthetic GPS channel/epoch scenario"); puts("stage epochs/fixes candidates->selected and position scatter"); for (unsigned stage = 0u; stage < SYNTHETIC_STAGE_COUNT; stage++) { synthetic_print_metrics(metrics[stage]); } CHECK(saw_week_rollover, "general scenario must cross a GPS week boundary"); CHECK(saw_sample_rollover, "general scenario must cross uint32 ABS_SAMPLE rollover"); CHECK(metrics[STAGE_BASELINE].fixes >= 98u, "baseline must produce stable fixes across time/sample rollover"); CHECK(metrics[STAGE_ONE_DROPOUT].fixes >= 58u, "one satellite dropout must not interrupt fixes"); CHECK(metrics[STAGE_THREE_DROPOUTS].fixes >= 58u, "five remaining satellites must continue producing fixes"); CHECK(metrics[STAGE_MINIMUM_FOUR].fixes == 40u, "four satellites must remain a publishable minimum fix"); CHECK(metrics[STAGE_OUTAGE_THREE].fixes == 0u && below_minimum_fixes == 0u, "three satellites must suppress the position solution"); CHECK(metrics[STAGE_REACQUIRE].fixes >= 58u, "position must recover after satellite reacquisition"); CHECK(delayed_selection_failures == 0u && metrics[STAGE_DELAYED_CHANNEL].fixes >= 58u, "delayed/newest outlier channel must be excluded without blocking fixes"); CHECK(precommit_join_count_failures == 0u, "joining channel must remain excluded before matching SF1/SF2/SF3"); CHECK(postcommit_join_count_failures == 0u, "joining channel must enter immediately after SF1/SF2/SF3 commit"); CHECK(metrics[STAGE_EPHEMERIS_JOIN].fixes >= 58u, "ephemeris-ready channel join must preserve position continuity"); CHECK(metrics[STAGE_PSEUDORANGE_FAULT].fixes >= 58u && metrics[STAGE_PSEUDORANGE_FAULT].fde_fixes >= 58u && fde_wrong_exclusion == 0u, "single 100 m pseudorange fault must be isolated by FDE"); CHECK(metrics[STAGE_BASELINE].horizontal_max_m < 25.0 && metrics[STAGE_REACQUIRE].horizontal_max_m < 25.0 && metrics[STAGE_DELAYED_CHANNEL].horizontal_max_m < 25.0, "nominal/recovery/delay position error must stay bounded"); CHECK(metrics[STAGE_MINIMUM_FOUR].horizontal_max_m < 120.0, "minimum four-satellite geometry must stay within the synthetic bound"); } int main(int argc, char** argv) { const unsigned satellite_count = 7; const double latitude_rad = 39.925 * PI / 180.0; const double longitude_rad = 32.837 * PI / 180.0; const double receiver_height_m = 890.0; const double clock_bias_m = 74000.0; double receiver_truth[3]; geodetic_to_ecef(latitude_rad, longitude_rad, receiver_height_m, receiver_truth); double seed[STATE_DIM] = { receiver_truth[0] + 120.0, receiver_truth[1] - 90.0, receiver_truth[2] + 60.0, clock_bias_m - 200.0 }; const double clean_noise[MAX_SATELLITES] = {1.2, -1.8, 0.6, 2.0, -1.0, 1.5, -2.2, 0.0, 0.0}; measurement_t clean[MAX_SATELLITES]; create_measurements(clean, satellite_count, receiver_truth, latitude_rad, longitude_rad, clock_bias_m, clean_noise); if (argc >= 2 && strncmp(argv[1], "--bench-", 8) == 0) { unsigned iterations = (argc >= 3) ? (unsigned)strtoul(argv[2], NULL, 10) : 100000u; if (iterations == 0u) { iterations = 1u; } return run_benchmark(argv[1] + 8, iterations, clean, satellite_count, seed, latitude_rad, longitude_rad); } puts("Synthetic standalone GPS integrity/FDE comparison"); puts("quality = 100/(1 + chi_square/threshold); 50 is the 99% reject boundary\n"); const double receiver_samples_per_second = RECEIVER_SAMPLES_PER_SECOND; int anchored_week = 0; double anchored_tow = 0.0; double anchor_age = 0.0; CHECK(gps_time_from_sample_anchor(2300, 46386.0, 100000000u, 99999999.75, 100000000u, 99999999.75, receiver_samples_per_second, 12.0, &anchored_week, &anchored_tow, &anchor_age) && anchored_week == 2300 && fabs(anchored_tow - 46386.0) < 1.0e-9, "simultaneous HOW/nav-solve event must have zero elapsed time"); CHECK(gps_time_from_sample_anchor(2300, 46386.0, 100000000u, 99999999.75, 103273600u, 103273599.75, receiver_samples_per_second, 12.0, &anchored_week, &anchored_tow, &anchor_age), "200 ms sample-anchor conversion should succeed"); CHECK(anchored_week == 2300 && fabs(anchored_tow - 46386.2) < 1.0e-9, "200 ms event must be derived from receiver samples, not NAV timer ownership"); /* A channel enabled later receives a newer absolute HOW/sample anchor. * Both channels must still calculate the same time for a common epoch. */ int late_week = 0; double late_tow = 0.0; CHECK(gps_time_from_sample_anchor(2300, 46392.0, 198208000u, 198207999.5, 214576000u, 214575999.5, receiver_samples_per_second, 12.0, &late_week, &late_tow, NULL), "late-enable channel anchor conversion should succeed"); int early_week = 0; double early_tow = 0.0; CHECK(gps_time_from_sample_anchor(2300, 46386.0, 100000000u, 99999999.5, 214576000u, 214575999.5, receiver_samples_per_second, 12.0, &early_week, &early_tow, NULL), "early channel anchor conversion should succeed"); CHECK(early_week == late_week && fabs(early_tow - late_tow) < 1.0e-9, "late-enable and early channels must agree at a common receiver sample"); CHECK(gps_time_from_sample_anchor(2300, 100.0, 500000000u, 499999999.75, 549104000u, 549103999.75, receiver_samples_per_second, 12.0, &anchored_week, &anchored_tow, &anchor_age) && fabs(anchored_tow - 103.0) < 1.0e-9, "coalesced/missed NAV events must not lose elapsed GPS time"); uint32_t wrap_anchor = UINT32_MAX - 8183999u; uint32_t wrap_event = wrap_anchor + 16368000u; CHECK(gps_time_from_sample_anchor(2300, 200.0, wrap_anchor, (double)wrap_anchor - 0.25, wrap_event, (double)wrap_event - 0.25, receiver_samples_per_second, 12.0, &anchored_week, &anchored_tow, NULL) && fabs(anchored_tow - 201.0) < 1.0e-9, "32-bit ABS_SAMPLE rollover must unwrap correctly"); CHECK(gps_time_from_sample_anchor(2300, 604794.0, 700000000u, 699999999.5, 814576000u, 814575999.5, receiver_samples_per_second, 12.0, &anchored_week, &anchored_tow, NULL) && anchored_week == 2301 && fabs(anchored_tow - 1.0) < 1.0e-9, "GPS week rollover must increment the anchor week"); CHECK(gps_time_resolve_how_week(true, 2300, 604794.0, 2300, 0.0) == 2301, "HOW seconds-of-week rollover must resolve to the next week"); CHECK(!gps_time_from_sample_anchor(2300, 100.0, 100000000u, 99999999.5, 312784001u, 312784000.5, receiver_samples_per_second, 12.0, &anchored_week, &anchored_tow, NULL), "stale HOW anchor must be rejected"); gps_nav_epoch_selection_t epoch_selection; gps_nav_epoch_key_t mismatched_epoch[MAX_SATELLITES] = { {2300, 46386.8, 1000000u}, {2300, 46386.8, 1010000u}, {2300, 46386.8, 1020000u}, {2300, 46386.8, 1030000u}, {2300, 46386.8, 1040000u}, {2300, 46386.6, 5000000u}, {2300, 46386.8, 1050000u} }; CHECK(gps_nav_epoch_select(mismatched_epoch, 7u, EPOCH_SAMPLE_WINDOW, EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) && epoch_selection.selected_count == 6u && (epoch_selection.selected_mask & (1u << 5)) == 0u, "200 ms TOW outlier must be excluded without blocking six good channels"); gps_nav_epoch_key_t coherent_epoch[MAX_SATELLITES] = { {2300, 46386.8, 1000000u}, {2300, 46386.8, 1010000u}, {2300, 46386.8, 1020000u}, {2300, 46386.8, 1030000u}, {2300, 46386.8, 1040000u}, {2300, 46386.8, 1050000u}, {2300, 46386.8, 1060000u} }; CHECK(gps_nav_epoch_select(coherent_epoch, 7u, EPOCH_SAMPLE_WINDOW, EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) && epoch_selection.selected_count == 7u, "fully coherent epoch must retain all channels"); gps_nav_epoch_key_t ambiguous_epoch[MAX_SATELLITES] = { {2300, 100.0, 1000000u}, {2300, 100.0, 1010000u}, {2300, 101.0, 5000000u}, {2300, 101.0, 5010000u} }; CHECK(gps_nav_epoch_select(ambiguous_epoch, 4u, EPOCH_SAMPLE_WINDOW, EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) && epoch_selection.selected_count == 2u, "two-versus-two epoch split must stay below the four-channel minimum"); gps_nav_epoch_key_t excessive_epoch[MAX_SATELLITES] = { {2300, 100.0, 1000000u}, {2300, 100.0, 1010000u}, {2300, 100.0, 1020000u}, {2300, 96.0, 5000000u} }; CHECK(gps_nav_epoch_select(excessive_epoch, 4u, EPOCH_SAMPLE_WINDOW, EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) && epoch_selection.selected_count == 3u, "three-versus-one disagreement must stay below the navigation minimum"); solution_t absolute_solution; solution_t normalized_solution; uint32_t full_mask = (1u << satellite_count) - 1u; CHECK(solve_subset(clean, satellite_count, full_mask, SOLVE_ABSOLUTE_WEIGHTS, seed, latitude_rad, longitude_rad, &absolute_solution), "absolute-weight clean solve"); CHECK(solve_subset(clean, satellite_count, full_mask, SOLVE_NORMALIZED_WEIGHTS, seed, latitude_rad, longitude_rad, &normalized_solution), "normalized-weight clean solve"); print_solution("clean absolute", &absolute_solution, receiver_truth); print_solution("clean normalized", &normalized_solution, receiver_truth); double normalization_state_difference[3] = { absolute_solution.state[0] - normalized_solution.state[0], absolute_solution.state[1] - normalized_solution.state[1], absolute_solution.state[2] - normalized_solution.state[2] }; CHECK(vector_norm3(normalization_state_difference) < 1.0e-5, "common weight normalization must not change WLS position"); CHECK(normalized_solution.chi_square_pass, "clean epoch should pass chi-square"); measurement_t single_fault[MAX_SATELLITES]; memcpy(single_fault, clean, sizeof(single_fault)); single_fault[2].pseudorange_m += 100.0; solution_t single_full; solution_t single_selected; CHECK(solve_with_fde(single_fault, satellite_count, seed, latitude_rad, longitude_rad, &single_selected, &single_full), "single-fault FDE solve"); print_solution("single fault: full", &single_full, receiver_truth); print_solution("single fault: selected", &single_selected, receiver_truth); CHECK(!single_full.chi_square_pass, "100 m fault should fail the global test"); CHECK(single_selected.chi_square_pass, "leave-one-out should recover single fault"); CHECK(single_selected.excluded_mask == (1u << 2), "FDE should exclude PRN 3 row"); CHECK(position_error_m(&single_selected, receiver_truth) < position_error_m(&single_full, receiver_truth), "single-fault FDE should improve position"); /* With six available satellites, exclusion leaves five measurements and * only one residual DOF. This deterministic nominal-noise epoch is a * regression for the large exclusion-switch jumps seen on hardware. */ const unsigned six_satellite_count = 6u; const double ambiguous_six_noise[MAX_SATELLITES] = {6.604, -11.586, 10.801, -5.068, 4.625, -22.908, 0.0, 0.0, 0.0}; measurement_t ambiguous_six[MAX_SATELLITES]; create_measurements(ambiguous_six, six_satellite_count, receiver_truth, latitude_rad, longitude_rad, clock_bias_m, ambiguous_six_noise); solution_t unsafe_six_selected; solution_t unsafe_six_full; solution_t guarded_six_selected; solution_t guarded_six_full; CHECK(solve_with_fde_minimum(ambiguous_six, six_satellite_count, 6u, seed, latitude_rad, longitude_rad, &unsafe_six_selected, &unsafe_six_full), "six-satellite legacy leave-one-out regression solve"); CHECK(solve_with_fde(ambiguous_six, six_satellite_count, seed, latitude_rad, longitude_rad, &guarded_six_selected, &guarded_six_full), "six-satellite guarded FDE solve"); print_solution("six sat: full", &unsafe_six_full, receiver_truth); print_solution("six sat: unsafe FDE", &unsafe_six_selected, receiver_truth); print_solution("six sat: guarded", &guarded_six_selected, receiver_truth); CHECK(!unsafe_six_full.chi_square_pass, "six-satellite regression epoch should fail the full global test"); CHECK(unsafe_six_selected.fde_applied && unsafe_six_selected.chi_square_pass, "unsafe six-satellite leave-one-out should demonstrate a false isolation"); CHECK(position_error_m(&unsafe_six_selected, receiver_truth) > position_error_m(&unsafe_six_full, receiver_truth) + 50.0, "unsafe six-satellite FDE should demonstrate the observed position regression"); CHECK(!guarded_six_selected.fde_applied, "six-satellite solution must not replace full WLS with a one-DOF subset"); CHECK(!guarded_six_selected.solution_valid, "failed six-satellite full solution must remain explicitly invalid"); CHECK(memcmp(guarded_six_selected.state, guarded_six_full.state, sizeof(guarded_six_selected.state)) == 0, "guarded six-satellite output must retain the legacy full-WLS position"); measurement_t double_fault[MAX_SATELLITES]; memcpy(double_fault, clean, sizeof(double_fault)); double_fault[2].pseudorange_m += 100.0; double_fault[5].pseudorange_m += 50.0; solution_t double_full; solution_t double_selected; CHECK(solve_with_fde(double_fault, satellite_count, seed, latitude_rad, longitude_rad, &double_selected, &double_full), "double-fault best-effort solve"); print_solution("two faults: full", &double_full, receiver_truth); print_solution("two faults: best effort", &double_selected, receiver_truth); CHECK(!double_full.chi_square_pass, "two-fault full solution should fail"); CHECK(!double_selected.chi_square_pass, "one exclusion must not claim integrity with a remaining fault"); CHECK(double_selected.chi_square_ratio < double_full.chi_square_ratio, "best-effort candidate should retain a measurable improvement"); CHECK(position_error_m(&double_selected, receiver_truth) < position_error_m(&double_full, receiver_truth), "chosen synthetic best-effort case should improve position without passing integrity"); CHECK(!double_selected.solution_valid, "improved but still inconsistent solution must remain operationally invalid"); double trusted_state[STATE_DIM]; memcpy(trusted_state, normalized_solution.state, sizeof(trusted_state)); if (double_selected.solution_valid) { memcpy(trusted_state, double_selected.state, sizeof(trusted_state)); } CHECK(memcmp(trusted_state, normalized_solution.state, sizeof(trusted_state)) == 0, "invalid best-effort solution must not update trusted last_solution/PPS state"); measurement_t low_elevation_fault[MAX_SATELLITES]; memcpy(low_elevation_fault, clean, sizeof(low_elevation_fault)); low_elevation_fault[6].pseudorange_m += 100.0; solution_t weighted_low_elevation; solution_t uniform_low_elevation; CHECK(solve_subset(low_elevation_fault, satellite_count, full_mask, SOLVE_NORMALIZED_WEIGHTS, seed, latitude_rad, longitude_rad, &weighted_low_elevation), "weighted low-elevation solve"); CHECK(solve_subset(low_elevation_fault, satellite_count, full_mask, SOLVE_UNIFORM_WEIGHTS, seed, latitude_rad, longitude_rad, &uniform_low_elevation), "uniform low-elevation solve"); print_solution("low-el fault: weighted", &weighted_low_elevation, receiver_truth); print_solution("low-el fault: uniform", &uniform_low_elevation, receiver_truth); CHECK(position_error_m(&weighted_low_elevation, receiver_truth) < position_error_m(&uniform_low_elevation, receiver_truth), "elevation weighting should reduce a low-elevation fault's position effect"); CHECK(normalized_solution.covariance_enu_m2[0][0] > 0.0 && normalized_solution.covariance_enu_m2[1][1] > 0.0 && normalized_solution.covariance_enu_m2[2][2] > 0.0, "ENU covariance diagonal must be positive"); run_general_synthetic_scenario(receiver_truth, latitude_rad, longitude_rad, clock_bias_m); if (failures == 0) { puts("\nPASS: all synthetic integrity checks passed"); return 0; } fprintf(stderr, "\n%d synthetic integrity check(s) failed\n", failures); return 1; }