onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
dce1dd5ea9ea7f0bf84edfb6b56c7824f4adf14f
gps/soc/software/fusion/tests/standalone_gps_integrity_test.c
T
onur 30179df8e6 feat(gps): add timer-based assisted reacquisition 
Remove the fixed pseudorange start offset, preserve per-PRN navigation assistance across RF loss and power cycles, and seed coherent navigation measurement phase on reacquisition. Add full LNAV almanac visibility scheduling, power-safe QSPI journaling, RF-domain reset isolation, and PPS Gray CDC hardening with Linux, sanitizer, RTL, and end-to-end coverage.
2026-08-04 02:58:11 +03:00

1704 lines
69 KiB
C
Raw Blame History

#define _POSIX_C_SOURCE 200809L
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include "gps/gps_nav_epoch.h"
#include "gps/gps_ephemeris_cache.h"
#include "gps/gps_time_anchor.h"
#define MAX_SATELLITES 9
#define STATE_DIM 4
#define TEST_PI 3.14159265358979323846
#define RECEIVER_SAMPLES_PER_SECOND 16368000.0
#define RECEIVER_COUNTER_PPM 0.20
#define SPEED_OF_LIGHT_MPS 299792458.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] * TEST_PI / 180.0;
double elevation = elevation_deg[index] * TEST_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;
unsigned ephemeris_decode_count;
} 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;
unsigned sample_observations;
unsigned ephemeris_cache_restores;
unsigned ephemeris_decodes;
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 * TEST_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->ephemeris_valid &&
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;
channel->ephemeris_decode_count++;
}
}
static bool linux_nav_cache_ephemeris(gps_ephemeris_cache_t* cache,
unsigned satellite,
unsigned char iod,
uint64_t tick)
{
gps_ephemeris_t ephemeris;
memset(&ephemeris, 0, sizeof(ephemeris));
ephemeris.SVID = (unsigned char)satellite;
ephemeris.iode = iod;
ephemeris.ephemeris_valid = true;
ephemeris.toe.week = 2300;
ephemeris.toe.sec = 604000.0;
return gps_ephemeris_cache_store(
cache, satellite, &ephemeris, tick);
}
static bool linux_nav_restore_ephemeris(gps_ephemeris_cache_t* cache,
linux_nav_channel_t* channel,
unsigned satellite,
uint64_t tick)
{
gps_ephemeris_t ephemeris;
if (!gps_ephemeris_cache_restore(
cache, satellite, tick, &ephemeris)) {
return false;
}
channel->ephemeris_valid = ephemeris.ephemeris_valid;
memset(channel->have_sf, 0, sizeof(channel->have_sf));
memset(channel->iod, 0, sizeof(channel->iod));
return channel->ephemeris_valid;
}
static bool synthetic_apply_receiver_sample_datum(
measurement_t measurements[MAX_SATELLITES],
unsigned count,
unsigned epoch,
int week,
double tow_sec,
double absolute_clock_bias_m,
double receiver_counter_ppm)
{
if (count == 0u) {
return false;
}
const double actual_sample_hz =
RECEIVER_SAMPLES_PER_SECOND *
(1.0 + receiver_counter_ppm * 1.0e-6);
/* Production unwraps every channel's shared 32-bit RF sample latch around
* the epoch reference. Keep an arbitrary moving origin here to prove that
* neither boot time nor a counter rollover is a range datum. */
const double epoch_sample = 1000000000.375 +
fmod((double)epoch * 0.2 * actual_sample_hz, 4294967296.0);
double capture_sample[MAX_SATELLITES];
for (unsigned satellite = 0u; satellite < count; satellite++) {
double propagation_sec =
(measurements[satellite].pseudorange_m - absolute_clock_bias_m) /
SPEED_OF_LIGHT_MPS;
if (!isfinite(propagation_sec) || propagation_sec < 0.0) {
return false;
}
capture_sample[satellite] = epoch_sample +
propagation_sec * actual_sample_hz;
}
for (unsigned satellite = 0u; satellite < count; satellite++) {
double relative_observation_sec = 0.0;
if (!gps_time_sample_differential_observation(
week, tow_sec, capture_sample[0],
week, tow_sec, capture_sample[satellite],
RECEIVER_SAMPLES_PER_SECOND,
&relative_observation_sec)) {
return false;
}
measurements[satellite].pseudorange_m =
relative_observation_sec * SPEED_OF_LIGHT_MPS;
}
return true;
}
static void verify_common_start_offset_cannot_change_position(
const measurement_t absolute_measurements[MAX_SATELLITES],
unsigned count,
const double receiver_truth[3],
double latitude_rad,
double longitude_rad)
{
measurement_t differential[MAX_SATELLITES];
measurement_t shifted[MAX_SATELLITES];
const double old_start_offset_m =
0.068802 * SPEED_OF_LIGHT_MPS;
const double reference_observation_m =
absolute_measurements[0].pseudorange_m;
for (unsigned row = 0u; row < count; row++) {
differential[row] = absolute_measurements[row];
shifted[row] = absolute_measurements[row];
differential[row].pseudorange_m -= reference_observation_m;
shifted[row].pseudorange_m =
differential[row].pseudorange_m + old_start_offset_m;
}
double differential_seed[STATE_DIM] = {
receiver_truth[0] + 120.0,
receiver_truth[1] - 90.0,
receiver_truth[2] + 60.0,
-20200000.0
};
double shifted_seed[STATE_DIM] = {
receiver_truth[0] + 120.0,
receiver_truth[1] - 90.0,
receiver_truth[2] + 60.0,
400000.0
};
solution_t differential_solution;
solution_t differential_full;
solution_t shifted_solution;
solution_t shifted_full;
bool differential_ok = solve_with_fde(
differential, count, differential_seed,
latitude_rad, longitude_rad,
&differential_solution, &differential_full);
bool shifted_ok = solve_with_fde(
shifted, count, shifted_seed,
latitude_rad, longitude_rad,
&shifted_solution, &shifted_full);
CHECK(differential_ok && shifted_ok &&
differential_solution.solution_valid &&
shifted_solution.solution_valid,
"common-datum WLS invariance solutions must be valid");
if (!differential_ok || !shifted_ok) {
return;
}
double position_delta[3] = {
shifted_solution.state[0] - differential_solution.state[0],
shifted_solution.state[1] - differential_solution.state[1],
shifted_solution.state[2] - differential_solution.state[2]
};
CHECK(vector_norm3(position_delta) < 1.0e-4,
"68.802 ms common offset changed WLS XYZ");
CHECK(fabs((shifted_solution.state[3] -
differential_solution.state[3]) -
old_start_offset_m) < 1.0e-4,
"68.802 ms common offset was not confined to WLS clock bias");
}
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 "
"sample=%4u cache=%2u decode=%2u "
"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.sample_observations,
metrics.ephemeris_cache_restores,
metrics.ephemeris_decodes,
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));
gps_ephemeris_cache_t ephemeris_cache;
gps_ephemeris_cache_init(&ephemeris_cache, 1000u);
for (unsigned satellite = 0u; satellite < 8u; satellite++) {
channels[satellite].ephemeris_valid = true;
channels[satellite].ephemeris_decode_count = 1u;
CHECK(linux_nav_cache_ephemeris(
&ephemeris_cache, satellite,
(unsigned char)(17u + satellite), 0u),
"initial per-PRN ephemeris cache store failed");
}
metrics[STAGE_BASELINE].ephemeris_decodes = 8u;
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,
-20200000.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;
unsigned sample_datum_failures = 0u;
unsigned ephemeris_cache_restore_total = 0u;
bool saw_week_rollover = false;
bool saw_sample_rollover = false;
uint32_t previous_raw_sample = 0u;
uint32_t previous_active_mask = 0x0ffu;
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++;
uint32_t active_mask = synthetic_active_mask(stage);
/* A tracking slot is volatile; the per-PRN cache is not. Model a
* building/bridge obstruction by clearing the channel copy on loss and
* restoring it from RAM exactly once when acquisition returns. */
for (unsigned satellite = 0u; satellite < MAX_SATELLITES; satellite++) {
bool was_active = (previous_active_mask & (1u << satellite)) != 0u;
bool is_active = (active_mask & (1u << satellite)) != 0u;
if (was_active && !is_active) {
channels[satellite].ephemeris_valid = false;
memset(channels[satellite].have_sf, 0,
sizeof(channels[satellite].have_sf));
memset(channels[satellite].iod, 0,
sizeof(channels[satellite].iod));
} else if (!was_active && is_active &&
linux_nav_restore_ephemeris(
&ephemeris_cache, &channels[satellite],
satellite, epoch)) {
stage_metrics->ephemeris_cache_restores++;
ephemeris_cache_restore_total++;
}
}
previous_active_mask = active_mask;
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);
if (channels[8].ephemeris_valid) {
CHECK(linux_nav_cache_ephemeris(
&ephemeris_cache, 8u, 29u, epoch),
"newly decoded PRN ephemeris was not cached");
stage_metrics->ephemeris_decodes++;
}
}
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;
}
if (!synthetic_apply_receiver_sample_datum(
master, MAX_SATELLITES, epoch, week, tow_sec,
clock_bias_m, RECEIVER_COUNTER_PPM)) {
sample_datum_failures++;
}
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;
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;
stage_metrics->sample_observations += candidate_count;
gps_nav_epoch_selection_t selection;
bool selected = gps_nav_epoch_validate_complete(
candidates, candidate_count, EPOCH_SAMPLE_WINDOW,
EPOCH_TOW_TOLERANCE_SEC, &selection);
unsigned selected_count = selection.selected_count;
stage_metrics->selected_total += selected_count;
stage_metrics->epoch_exclusions += candidate_count - selected_count;
if (stage == STAGE_DELAYED_CHANNEL &&
(selected ||
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 || 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 sample/cache/decode telemetry 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(sample_datum_failures == 0u,
"free-running RF sample counter failed to form an end-to-end observation");
CHECK(ephemeris_cache_restore_total == 5u,
"every returning cached PRN must restore exactly once");
CHECK(metrics[STAGE_REACQUIRE].ephemeris_cache_restores == 4u &&
metrics[STAGE_REACQUIRE].ephemeris_decodes == 0u,
"obstruction recovery must restore RAM ephemeris without SF1/SF2/SF3 decode");
CHECK(channels[8].ephemeris_decode_count == 1u &&
metrics[STAGE_EPHEMERIS_JOIN].ephemeris_decodes == 1u,
"an uncached PRN must join only after one matching ephemeris decode");
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 raw 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 == 0u,
"incoherent delayed channel must reject the complete raw epoch");
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");
}
typedef struct {
const char* name;
unsigned outage_sec;
uint32_t dropped_mask;
uint64_t cache_max_age_sec;
unsigned expected_cache_restores;
unsigned expected_reparses;
double receiver_counter_ppm;
} counter_reacquisition_case_t;
static unsigned synthetic_mask_count(uint32_t mask)
{
unsigned count = 0u;
while (mask != 0u) {
count += mask & 1u;
mask >>= 1u;
}
return count;
}
static bool synthetic_solve_counter_epoch(
unsigned epoch,
uint32_t active_mask,
const double receiver_truth[3],
double latitude_rad,
double longitude_rad,
double receiver_counter_ppm,
double* position_error_out)
{
const double no_noise[MAX_SATELLITES] = {0.0};
measurement_t all_measurements[MAX_SATELLITES];
create_measurements(all_measurements, 7u, receiver_truth,
latitude_rad, longitude_rad, 74000.0, no_noise);
int week = 0;
double tow_sec = 0.0;
synthetic_epoch_time(epoch, &week, &tow_sec);
if (!synthetic_apply_receiver_sample_datum(
all_measurements, 7u, epoch, week, tow_sec, 74000.0,
receiver_counter_ppm)) {
return false;
}
measurement_t selected[MAX_SATELLITES];
unsigned count = 0u;
for (unsigned satellite = 0u; satellite < 7u; satellite++) {
if ((active_mask & (1u << satellite)) != 0u) {
selected[count++] = all_measurements[satellite];
}
}
if (count < STATE_DIM) {
return false;
}
double seed[STATE_DIM] = {
receiver_truth[0] + 120.0,
receiver_truth[1] - 90.0,
receiver_truth[2] + 60.0,
-20200000.0
};
solution_t solution;
solution_t full_solution;
if (!solve_with_fde(selected, count, seed,
latitude_rad, longitude_rad,
&solution, &full_solution) ||
!solution.solution_valid) {
return false;
}
if (position_error_out != NULL) {
*position_error_out = position_error_m(&solution, receiver_truth);
}
return true;
}
static void run_counter_reacquisition_matrix(const double receiver_truth[3],
double latitude_rad,
double longitude_rad)
{
static const counter_reacquisition_case_t cases[] = {
{"tunnel-all-30s", 30u, 0x07fu, 7200u, 7u, 0u, 0.2},
{"drop-6of7-15s", 15u, 0x07eu, 7200u, 6u, 0u, 0.2},
{"drop-6of7-2s", 2u, 0x07eu, 7200u, 6u, 0u, 0.2},
{"drop-1of7-15s", 15u, 0x040u, 7200u, 1u, 0u, 0.2},
{"stale-all-7201s", 7201u, 0x07fu, 7200u, 0u, 7u, 0.2},
{"tunnel-30s-15ppm", 30u, 0x07fu, 7200u, 7u, 0u, 15.0}
};
const uint32_t full_mask = 0x07fu;
puts("\nRF free-counter + PPS epoch + per-PRN RAM reacquisition matrix");
puts("scenario outage drop remain ppm outage_fix cache reparse nav_wait recovery_error");
for (unsigned case_index = 0u;
case_index < sizeof(cases) / sizeof(cases[0]);
case_index++) {
const counter_reacquisition_case_t* scenario = &cases[case_index];
gps_ephemeris_cache_t cache;
gps_ephemeris_cache_init(&cache, scenario->cache_max_age_sec);
linux_nav_channel_t channels[7];
memset(channels, 0, sizeof(channels));
for (unsigned satellite = 0u; satellite < 7u; satellite++) {
channels[satellite].ephemeris_valid = true;
channels[satellite].ephemeris_decode_count = 1u;
CHECK(linux_nav_cache_ephemeris(
&cache, satellite,
(unsigned char)(40u + satellite), 0u),
"reacquisition matrix cache initialization failed");
}
uint32_t outage_active_mask =
full_mask & ~scenario->dropped_mask;
for (unsigned satellite = 0u; satellite < 7u; satellite++) {
if ((scenario->dropped_mask & (1u << satellite)) != 0u) {
channels[satellite].ephemeris_valid = false;
}
}
unsigned remain = synthetic_mask_count(outage_active_mask);
double outage_error_m = NAN;
bool outage_fix = synthetic_solve_counter_epoch(
(scenario->outage_sec * 5u) / 2u,
outage_active_mask, receiver_truth,
latitude_rad, longitude_rad,
scenario->receiver_counter_ppm, &outage_error_m);
CHECK(outage_fix == (remain >= STATE_DIM),
"outage fix availability did not match the four-satellite minimum");
if (outage_fix) {
CHECK(outage_error_m < 25.0,
"partial-outage RF-counter solution exceeded the recovery bound");
}
unsigned cache_restores = 0u;
unsigned reparses = 0u;
for (unsigned satellite = 0u; satellite < 7u; satellite++) {
if ((scenario->dropped_mask & (1u << satellite)) == 0u) {
continue;
}
if (linux_nav_restore_ephemeris(
&cache, &channels[satellite], satellite,
scenario->outage_sec)) {
cache_restores++;
continue;
}
/* Expired/missing RAM data follows the real matching-IOD commit
* rule; a partial or mismatched set is never made usable. */
linux_nav_receive_ephemeris_subframe(
&channels[satellite], 1u, (unsigned char)(70u + satellite));
linux_nav_receive_ephemeris_subframe(
&channels[satellite], 2u, (unsigned char)(70u + satellite));
linux_nav_receive_ephemeris_subframe(
&channels[satellite], 3u, (unsigned char)(70u + satellite));
if (channels[satellite].ephemeris_valid) {
reparses++;
CHECK(linux_nav_cache_ephemeris(
&cache, satellite,
(unsigned char)(70u + satellite),
scenario->outage_sec + 18u),
"fresh post-expiry ephemeris was not returned to RAM");
}
}
CHECK(cache_restores == scenario->expected_cache_restores,
"reacquisition matrix restored the wrong number of cached PRNs");
CHECK(reparses == scenario->expected_reparses,
"reacquisition matrix reparsed the wrong number of PRNs");
unsigned nav_wait_sec = (reparses == 0u) ? 6u : 18u;
unsigned recovery_epoch =
(scenario->outage_sec + nav_wait_sec) * 5u;
double recovery_error_m = NAN;
bool recovery_fix = synthetic_solve_counter_epoch(
recovery_epoch, full_mask, receiver_truth,
latitude_rad, longitude_rad,
scenario->receiver_counter_ppm, &recovery_error_m);
CHECK(recovery_fix && recovery_error_m < 25.0,
"post-reacquisition RF-counter position did not recover correctly");
printf("%-20s %6u %4u %6u %5.1f %10s %5u %7u %8u %14.3f m\n",
scenario->name,
scenario->outage_sec,
synthetic_mask_count(scenario->dropped_mask),
remain,
scenario->receiver_counter_ppm,
outage_fix ? "valid" : "suppressed",
cache_restores,
reparses,
nav_wait_sec,
recovery_error_m);
}
}
int main(int argc, char** argv)
{
const unsigned satellite_count = 7;
const double latitude_rad = 39.925 * TEST_PI / 180.0;
const double longitude_rad = 32.837 * TEST_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);
}
verify_common_start_offset_cannot_change_position(
clean, satellite_count, receiver_truth,
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);
run_counter_reacquisition_matrix(receiver_truth,
latitude_rad, longitude_rad);
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;
}
Reference in New Issue View Git Blame Copy Permalink