#define _POSIX_C_SOURCE 200809L #include #include #include #include #include #include #include #include #include "gps_time_anchor.h" #define MAX_SATELLITES 9 #define STATE_DIM 4 #define PI 3.14159265358979323846 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; typedef struct { int week; double sec; } test_tow_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 test_tow_delta_sec(const test_tow_t* first, const test_tow_t* second) { double delta = (double)(first->week - second->week) * 604800.0 + first->sec - second->sec; if (delta > 302400.0) { delta -= 604800.0; } else if (delta < -302400.0) { delta += 604800.0; } return delta; } static bool test_validate_tow(const test_tow_t tow[MAX_SATELLITES], unsigned count) { unsigned best_cluster = 0; for (unsigned candidate = 0; candidate < count; candidate++) { unsigned cluster = 0; for (unsigned row = 0; row < count; row++) { if (fabs(test_tow_delta_sec(&tow[row], &tow[candidate])) <= 0.001) { cluster++; } } if (cluster > best_cluster) { best_cluster = cluster; } } if (best_cluster <= count / 2u) { return false; } return best_cluster == count; } 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->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; } 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 = 16368000.0; 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"); test_tow_t mismatched_tow[MAX_SATELLITES] = { {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.6}, {2300, 46386.8} }; CHECK(!test_validate_tow(mismatched_tow, 7u), "200 ms TOW mismatch must reject instead of copying the majority TOW"); test_tow_t coherent_tow[MAX_SATELLITES] = { {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8} }; CHECK(test_validate_tow(coherent_tow, 7u), "fully coherent TOW snapshot should pass"); test_tow_t ambiguous_tow[MAX_SATELLITES] = { {2300, 100.0}, {2300, 100.0}, {2300, 101.0}, {2300, 101.0} }; CHECK(!test_validate_tow(ambiguous_tow, 4u), "two-versus-two TOW split must reject the navigation epoch"); test_tow_t excessive_tow[MAX_SATELLITES] = { {2300, 100.0}, {2300, 100.0}, {2300, 100.0}, {2300, 96.0} }; CHECK(!test_validate_tow(excessive_tow, 4u), "large TOW disagreement must not be silently corrected"); 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"); 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; }