onur
516016efb9
Wait for every navigation-ready tracking channel in a GPS epoch, remove unlocked channels without a timeout, and keep late joins for the next epoch. Reset PLIC ownership during warm initialization and extend Linux synthetic, integrity, and fake-MMIO regression coverage.
1296 lines
53 KiB
C
1296 lines
53 KiB
C
#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_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;
|
|
}
|