865 lines
36 KiB
C
865 lines
36 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_time_anchor.h"
|
|
|
|
#define MAX_SATELLITES 9
|
|
#define STATE_DIM 4
|
|
#define PI 3.14159265358979323846
|
|
|
|
typedef struct {
|
|
unsigned prn;
|
|
double position_ecef_m[3];
|
|
double pseudorange_m;
|
|
double sigma_m;
|
|
} measurement_t;
|
|
|
|
typedef struct {
|
|
double state[STATE_DIM];
|
|
double covariance_enu_m2[3][3];
|
|
double hacc_m;
|
|
double vacc_m;
|
|
double residual_rms_m;
|
|
double weighted_residual_rms_m;
|
|
double chi_square;
|
|
double chi_square_threshold;
|
|
double chi_square_ratio;
|
|
double quality_score;
|
|
unsigned used_count;
|
|
unsigned dof;
|
|
uint32_t used_mask;
|
|
uint32_t excluded_mask;
|
|
bool residual_test_valid;
|
|
bool chi_square_pass;
|
|
bool fde_applied;
|
|
bool solution_valid;
|
|
} solution_t;
|
|
|
|
typedef enum {
|
|
SOLVE_ABSOLUTE_WEIGHTS,
|
|
SOLVE_NORMALIZED_WEIGHTS,
|
|
SOLVE_UNIFORM_WEIGHTS
|
|
} solve_weight_mode_t;
|
|
|
|
typedef struct {
|
|
int week;
|
|
double sec;
|
|
} test_tow_t;
|
|
|
|
static int failures;
|
|
static volatile double benchmark_sink;
|
|
|
|
#define CHECK(condition, message) do { \
|
|
if (!(condition)) { \
|
|
fprintf(stderr, "FAIL: %s\n", message); \
|
|
failures++; \
|
|
} \
|
|
} while (0)
|
|
|
|
static double square(double x)
|
|
{
|
|
return x * x;
|
|
}
|
|
|
|
static double test_tow_delta_sec(const test_tow_t* first, const test_tow_t* second)
|
|
{
|
|
double delta = (double)(first->week - second->week) * 604800.0 +
|
|
first->sec - second->sec;
|
|
if (delta > 302400.0) {
|
|
delta -= 604800.0;
|
|
} else if (delta < -302400.0) {
|
|
delta += 604800.0;
|
|
}
|
|
return delta;
|
|
}
|
|
|
|
static bool test_validate_tow(const test_tow_t tow[MAX_SATELLITES], unsigned count)
|
|
{
|
|
unsigned best_cluster = 0;
|
|
for (unsigned candidate = 0; candidate < count; candidate++) {
|
|
unsigned cluster = 0;
|
|
for (unsigned row = 0; row < count; row++) {
|
|
if (fabs(test_tow_delta_sec(&tow[row], &tow[candidate])) <= 0.001) {
|
|
cluster++;
|
|
}
|
|
}
|
|
if (cluster > best_cluster) {
|
|
best_cluster = cluster;
|
|
}
|
|
}
|
|
if (best_cluster <= count / 2u) {
|
|
return false;
|
|
}
|
|
return best_cluster == count;
|
|
}
|
|
|
|
static double vector_norm3(const double v[3])
|
|
{
|
|
return sqrt(square(v[0]) + square(v[1]) + square(v[2]));
|
|
}
|
|
|
|
static double position_error_m(const solution_t* solution, const double truth[3])
|
|
{
|
|
double error[3] = {
|
|
solution->state[0] - truth[0],
|
|
solution->state[1] - truth[1],
|
|
solution->state[2] - truth[2]
|
|
};
|
|
return vector_norm3(error);
|
|
}
|
|
|
|
static bool inverse4(double input[STATE_DIM][STATE_DIM],
|
|
double inverse[STATE_DIM][STATE_DIM])
|
|
{
|
|
double augmented[STATE_DIM][2 * STATE_DIM];
|
|
double scale = 0.0;
|
|
|
|
for (unsigned row = 0; row < STATE_DIM; row++) {
|
|
for (unsigned col = 0; col < STATE_DIM; col++) {
|
|
augmented[row][col] = input[row][col];
|
|
augmented[row][col + STATE_DIM] = (row == col) ? 1.0 : 0.0;
|
|
if (fabs(input[row][col]) > scale) {
|
|
scale = fabs(input[row][col]);
|
|
}
|
|
}
|
|
}
|
|
if (!(scale > 0.0) || !isfinite(scale)) {
|
|
return false;
|
|
}
|
|
|
|
for (unsigned pivot = 0; pivot < STATE_DIM; pivot++) {
|
|
unsigned pivot_row = pivot;
|
|
double pivot_magnitude = fabs(augmented[pivot][pivot]);
|
|
for (unsigned row = pivot + 1; row < STATE_DIM; row++) {
|
|
double magnitude = fabs(augmented[row][pivot]);
|
|
if (magnitude > pivot_magnitude) {
|
|
pivot_magnitude = magnitude;
|
|
pivot_row = row;
|
|
}
|
|
}
|
|
if (pivot_magnitude <= scale * DBL_EPSILON * 128.0) {
|
|
return false;
|
|
}
|
|
if (pivot_row != pivot) {
|
|
for (unsigned col = 0; col < 2 * STATE_DIM; col++) {
|
|
double temporary = augmented[pivot][col];
|
|
augmented[pivot][col] = augmented[pivot_row][col];
|
|
augmented[pivot_row][col] = temporary;
|
|
}
|
|
}
|
|
|
|
double divisor = augmented[pivot][pivot];
|
|
for (unsigned col = 0; col < 2 * STATE_DIM; col++) {
|
|
augmented[pivot][col] /= divisor;
|
|
}
|
|
for (unsigned row = 0; row < STATE_DIM; row++) {
|
|
if (row == pivot) {
|
|
continue;
|
|
}
|
|
double factor = augmented[row][pivot];
|
|
for (unsigned col = 0; col < 2 * STATE_DIM; col++) {
|
|
augmented[row][col] -= factor * augmented[pivot][col];
|
|
}
|
|
}
|
|
}
|
|
|
|
for (unsigned row = 0; row < STATE_DIM; row++) {
|
|
for (unsigned col = 0; col < STATE_DIM; col++) {
|
|
inverse[row][col] = augmented[row][col + STATE_DIM];
|
|
if (!isfinite(inverse[row][col])) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool solve_normal_equations(double normal[STATE_DIM][STATE_DIM],
|
|
const double rhs[STATE_DIM],
|
|
double update[STATE_DIM])
|
|
{
|
|
double inverse[STATE_DIM][STATE_DIM];
|
|
if (!inverse4(normal, inverse)) {
|
|
return false;
|
|
}
|
|
for (unsigned row = 0; row < STATE_DIM; row++) {
|
|
update[row] = 0.0;
|
|
for (unsigned col = 0; col < STATE_DIM; col++) {
|
|
update[row] += inverse[row][col] * rhs[col];
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static double chi_square_threshold_99(unsigned dof)
|
|
{
|
|
static const double thresholds[] = {
|
|
0.0,
|
|
6.634896601,
|
|
9.210340372,
|
|
11.34486673,
|
|
13.27670414,
|
|
15.08627247
|
|
};
|
|
return (dof < (sizeof(thresholds) / sizeof(thresholds[0])))
|
|
? thresholds[dof] : 0.0;
|
|
}
|
|
|
|
static void geodetic_to_ecef(double latitude_rad, double longitude_rad,
|
|
double height_m, double ecef_m[3])
|
|
{
|
|
const double semi_major_axis_m = 6378137.0;
|
|
const double eccentricity_squared = 6.6943799901413165e-3;
|
|
double sin_latitude = sin(latitude_rad);
|
|
double cos_latitude = cos(latitude_rad);
|
|
double prime_vertical_radius = semi_major_axis_m /
|
|
sqrt(1.0 - eccentricity_squared * square(sin_latitude));
|
|
|
|
ecef_m[0] = (prime_vertical_radius + height_m) * cos_latitude * cos(longitude_rad);
|
|
ecef_m[1] = (prime_vertical_radius + height_m) * cos_latitude * sin(longitude_rad);
|
|
ecef_m[2] = (prime_vertical_radius * (1.0 - eccentricity_squared) + height_m) * sin_latitude;
|
|
}
|
|
|
|
static void enu_axes(double latitude_rad, double longitude_rad, double axes[3][3])
|
|
{
|
|
double sin_latitude = sin(latitude_rad);
|
|
double cos_latitude = cos(latitude_rad);
|
|
double sin_longitude = sin(longitude_rad);
|
|
double cos_longitude = cos(longitude_rad);
|
|
|
|
axes[0][0] = -sin_longitude;
|
|
axes[0][1] = cos_longitude;
|
|
axes[0][2] = 0.0;
|
|
axes[1][0] = -sin_latitude * cos_longitude;
|
|
axes[1][1] = -sin_latitude * sin_longitude;
|
|
axes[1][2] = cos_latitude;
|
|
axes[2][0] = cos_latitude * cos_longitude;
|
|
axes[2][1] = cos_latitude * sin_longitude;
|
|
axes[2][2] = sin_latitude;
|
|
}
|
|
|
|
static void create_measurements(measurement_t measurements[MAX_SATELLITES],
|
|
unsigned count,
|
|
const double receiver_ecef_m[3],
|
|
double latitude_rad,
|
|
double longitude_rad,
|
|
double receiver_clock_bias_m,
|
|
const double noise_m[MAX_SATELLITES])
|
|
{
|
|
static const double azimuth_deg[MAX_SATELLITES] =
|
|
{5.0, 52.0, 104.0, 158.0, 214.0, 267.0, 318.0, 74.0, 242.0};
|
|
static const double elevation_deg[MAX_SATELLITES] =
|
|
{72.0, 46.0, 31.0, 57.0, 26.0, 41.0, 12.0, 20.0, 35.0};
|
|
double axes[3][3];
|
|
enu_axes(latitude_rad, longitude_rad, axes);
|
|
|
|
for (unsigned index = 0; index < count; index++) {
|
|
double azimuth = azimuth_deg[index] * PI / 180.0;
|
|
double elevation = elevation_deg[index] * PI / 180.0;
|
|
double los_enu[3] = {
|
|
cos(elevation) * sin(azimuth),
|
|
cos(elevation) * cos(azimuth),
|
|
sin(elevation)
|
|
};
|
|
double los_ecef[3] = {0.0, 0.0, 0.0};
|
|
for (unsigned component = 0; component < 3; component++) {
|
|
for (unsigned enu = 0; enu < 3; enu++) {
|
|
los_ecef[component] += axes[enu][component] * los_enu[enu];
|
|
}
|
|
}
|
|
|
|
double geometric_range_m = 20200000.0 + 170000.0 * index;
|
|
measurements[index].prn = index + 1;
|
|
for (unsigned component = 0; component < 3; component++) {
|
|
measurements[index].position_ecef_m[component] =
|
|
receiver_ecef_m[component] + geometric_range_m * los_ecef[component];
|
|
}
|
|
double elevation_sigma_m = 3.0 / sin(elevation);
|
|
measurements[index].sigma_m = sqrt(square(2.4) + square(elevation_sigma_m));
|
|
measurements[index].pseudorange_m = geometric_range_m +
|
|
receiver_clock_bias_m + noise_m[index];
|
|
}
|
|
}
|
|
|
|
static bool solve_subset(const measurement_t measurements[MAX_SATELLITES],
|
|
unsigned available_count,
|
|
uint32_t used_mask,
|
|
solve_weight_mode_t weight_mode,
|
|
const double seed[STATE_DIM],
|
|
double latitude_rad,
|
|
double longitude_rad,
|
|
solution_t* output)
|
|
{
|
|
memset(output, 0, sizeof(*output));
|
|
output->used_mask = used_mask;
|
|
output->excluded_mask = ((1u << available_count) - 1u) & ~used_mask;
|
|
for (unsigned index = 0; index < available_count; index++) {
|
|
if ((used_mask & (1u << index)) != 0u) {
|
|
output->used_count++;
|
|
}
|
|
}
|
|
if (output->used_count < STATE_DIM) {
|
|
return false;
|
|
}
|
|
memcpy(output->state, seed, sizeof(output->state));
|
|
|
|
for (unsigned iteration = 0; iteration < 20; iteration++) {
|
|
double normal[STATE_DIM][STATE_DIM] = {{0.0}};
|
|
double rhs[STATE_DIM] = {0.0};
|
|
double maximum_absolute_weight = 0.0;
|
|
for (unsigned index = 0; index < available_count; index++) {
|
|
if ((used_mask & (1u << index)) == 0u) {
|
|
continue;
|
|
}
|
|
double absolute_weight = 1.0 / square(measurements[index].sigma_m);
|
|
if (absolute_weight > maximum_absolute_weight) {
|
|
maximum_absolute_weight = absolute_weight;
|
|
}
|
|
}
|
|
|
|
for (unsigned index = 0; index < available_count; index++) {
|
|
if ((used_mask & (1u << index)) == 0u) {
|
|
continue;
|
|
}
|
|
double receiver_to_satellite[3];
|
|
for (unsigned component = 0; component < 3; component++) {
|
|
receiver_to_satellite[component] =
|
|
output->state[component] - measurements[index].position_ecef_m[component];
|
|
}
|
|
double geometric_range_m = vector_norm3(receiver_to_satellite);
|
|
if (!(geometric_range_m > 1.0)) {
|
|
return false;
|
|
}
|
|
double design_row[STATE_DIM] = {
|
|
receiver_to_satellite[0] / geometric_range_m,
|
|
receiver_to_satellite[1] / geometric_range_m,
|
|
receiver_to_satellite[2] / geometric_range_m,
|
|
1.0
|
|
};
|
|
double residual_m = measurements[index].pseudorange_m -
|
|
geometric_range_m - output->state[3];
|
|
double absolute_weight = 1.0 / square(measurements[index].sigma_m);
|
|
double solve_weight = absolute_weight;
|
|
if (weight_mode == SOLVE_NORMALIZED_WEIGHTS) {
|
|
solve_weight /= maximum_absolute_weight;
|
|
} else if (weight_mode == SOLVE_UNIFORM_WEIGHTS) {
|
|
solve_weight = 1.0;
|
|
}
|
|
for (unsigned row = 0; row < STATE_DIM; row++) {
|
|
rhs[row] += solve_weight * design_row[row] * residual_m;
|
|
for (unsigned col = 0; col < STATE_DIM; col++) {
|
|
normal[row][col] += solve_weight * design_row[row] * design_row[col];
|
|
}
|
|
}
|
|
}
|
|
|
|
double update[STATE_DIM];
|
|
if (!solve_normal_equations(normal, rhs, update)) {
|
|
return false;
|
|
}
|
|
for (unsigned index = 0; index < STATE_DIM; index++) {
|
|
output->state[index] += update[index];
|
|
}
|
|
double position_update[3] = {update[0], update[1], update[2]};
|
|
if (vector_norm3(position_update) < 1.0e-5 && fabs(update[3]) < 1.0e-5) {
|
|
break;
|
|
}
|
|
if (iteration == 19) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
double absolute_normal[STATE_DIM][STATE_DIM] = {{0.0}};
|
|
double residual_square_sum = 0.0;
|
|
double absolute_weight_sum = 0.0;
|
|
output->chi_square = 0.0;
|
|
for (unsigned index = 0; index < available_count; index++) {
|
|
if ((used_mask & (1u << index)) == 0u) {
|
|
continue;
|
|
}
|
|
double receiver_to_satellite[3];
|
|
for (unsigned component = 0; component < 3; component++) {
|
|
receiver_to_satellite[component] =
|
|
output->state[component] - measurements[index].position_ecef_m[component];
|
|
}
|
|
double geometric_range_m = vector_norm3(receiver_to_satellite);
|
|
double design_row[STATE_DIM] = {
|
|
receiver_to_satellite[0] / geometric_range_m,
|
|
receiver_to_satellite[1] / geometric_range_m,
|
|
receiver_to_satellite[2] / geometric_range_m,
|
|
1.0
|
|
};
|
|
double residual_m = measurements[index].pseudorange_m -
|
|
geometric_range_m - output->state[3];
|
|
double absolute_weight = 1.0 / square(measurements[index].sigma_m);
|
|
residual_square_sum += square(residual_m);
|
|
output->chi_square += absolute_weight * square(residual_m);
|
|
absolute_weight_sum += absolute_weight;
|
|
for (unsigned row = 0; row < STATE_DIM; row++) {
|
|
for (unsigned col = 0; col < STATE_DIM; col++) {
|
|
absolute_normal[row][col] +=
|
|
absolute_weight * design_row[row] * design_row[col];
|
|
}
|
|
}
|
|
}
|
|
|
|
output->residual_rms_m = sqrt(residual_square_sum / output->used_count);
|
|
output->weighted_residual_rms_m = sqrt(output->chi_square / absolute_weight_sum);
|
|
output->dof = output->used_count - STATE_DIM;
|
|
output->chi_square_threshold = chi_square_threshold_99(output->dof);
|
|
output->residual_test_valid = output->dof > 0 && output->chi_square_threshold > 0.0;
|
|
if (output->residual_test_valid) {
|
|
output->chi_square_ratio = output->chi_square / output->chi_square_threshold;
|
|
output->quality_score = 100.0 / (1.0 + output->chi_square_ratio);
|
|
output->chi_square_pass = output->chi_square <= output->chi_square_threshold;
|
|
} else {
|
|
output->chi_square_ratio = INFINITY;
|
|
output->quality_score = 0.0;
|
|
}
|
|
output->solution_valid = output->residual_test_valid && output->chi_square_pass;
|
|
|
|
double covariance_ecef[STATE_DIM][STATE_DIM];
|
|
if (!inverse4(absolute_normal, covariance_ecef)) {
|
|
return false;
|
|
}
|
|
double axes[3][3];
|
|
enu_axes(latitude_rad, longitude_rad, axes);
|
|
for (unsigned enu_row = 0; enu_row < 3; enu_row++) {
|
|
for (unsigned enu_col = 0; enu_col < 3; enu_col++) {
|
|
double covariance = 0.0;
|
|
for (unsigned ecef_row = 0; ecef_row < 3; ecef_row++) {
|
|
for (unsigned ecef_col = 0; ecef_col < 3; ecef_col++) {
|
|
covariance += axes[enu_row][ecef_row] *
|
|
covariance_ecef[ecef_row][ecef_col] *
|
|
axes[enu_col][ecef_col];
|
|
}
|
|
}
|
|
output->covariance_enu_m2[enu_row][enu_col] = covariance;
|
|
}
|
|
}
|
|
output->hacc_m = sqrt(output->covariance_enu_m2[0][0] +
|
|
output->covariance_enu_m2[1][1]);
|
|
output->vacc_m = sqrt(output->covariance_enu_m2[2][2]);
|
|
return isfinite(output->hacc_m) && isfinite(output->vacc_m);
|
|
}
|
|
|
|
static bool solve_with_fde_minimum(const measurement_t measurements[MAX_SATELLITES],
|
|
unsigned count,
|
|
unsigned minimum_available_for_fde,
|
|
const double seed[STATE_DIM],
|
|
double latitude_rad,
|
|
double longitude_rad,
|
|
solution_t* selected,
|
|
solution_t* full_solution)
|
|
{
|
|
uint32_t full_mask = (1u << count) - 1u;
|
|
if (!solve_subset(measurements, count, full_mask, SOLVE_NORMALIZED_WEIGHTS,
|
|
seed, latitude_rad, longitude_rad, full_solution)) {
|
|
return false;
|
|
}
|
|
*selected = *full_solution;
|
|
if (full_solution->chi_square_pass || count < minimum_available_for_fde) {
|
|
return true;
|
|
}
|
|
|
|
bool passing_candidate_found = false;
|
|
for (unsigned excluded = 0; excluded < count; excluded++) {
|
|
solution_t candidate;
|
|
uint32_t candidate_mask = full_mask & ~(1u << excluded);
|
|
if (!solve_subset(measurements, count, candidate_mask, SOLVE_NORMALIZED_WEIGHTS,
|
|
full_solution->state, latitude_rad, longitude_rad, &candidate)) {
|
|
continue;
|
|
}
|
|
|
|
bool choose = false;
|
|
if (candidate.chi_square_pass) {
|
|
choose = !passing_candidate_found ||
|
|
candidate.chi_square_ratio < selected->chi_square_ratio;
|
|
} else if (!passing_candidate_found &&
|
|
candidate.chi_square_ratio < selected->chi_square_ratio) {
|
|
choose = true;
|
|
}
|
|
if (choose) {
|
|
*selected = candidate;
|
|
selected->fde_applied = true;
|
|
}
|
|
if (candidate.chi_square_pass) {
|
|
passing_candidate_found = true;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool solve_with_fde(const measurement_t measurements[MAX_SATELLITES],
|
|
unsigned count,
|
|
const double seed[STATE_DIM],
|
|
double latitude_rad,
|
|
double longitude_rad,
|
|
solution_t* selected,
|
|
solution_t* full_solution)
|
|
{
|
|
return solve_with_fde_minimum(measurements, count, 7u, seed,
|
|
latitude_rad, longitude_rad,
|
|
selected, full_solution);
|
|
}
|
|
|
|
static void print_solution(const char* label,
|
|
const solution_t* solution,
|
|
const double receiver_truth[3])
|
|
{
|
|
printf("%-24s used=%u excluded=0x%03x error=%7.2f m residual=%7.2f m "
|
|
"chi2=%8.2f dof=%u ratio=%6.2f quality=%6.1f hAcc=%5.2f vAcc=%5.2f "
|
|
"pass=%u valid=%u\n",
|
|
label,
|
|
solution->used_count,
|
|
solution->excluded_mask,
|
|
position_error_m(solution, receiver_truth),
|
|
solution->residual_rms_m,
|
|
solution->chi_square,
|
|
solution->dof,
|
|
solution->chi_square_ratio,
|
|
solution->quality_score,
|
|
solution->hacc_m,
|
|
solution->vacc_m,
|
|
solution->chi_square_pass,
|
|
solution->solution_valid);
|
|
}
|
|
|
|
static double monotonic_seconds(void)
|
|
{
|
|
struct timespec now;
|
|
clock_gettime(CLOCK_MONOTONIC, &now);
|
|
return (double)now.tv_sec + (double)now.tv_nsec * 1.0e-9;
|
|
}
|
|
|
|
static int run_benchmark(const char* scenario,
|
|
unsigned iterations,
|
|
const measurement_t clean[MAX_SATELLITES],
|
|
unsigned satellite_count,
|
|
const double seed[STATE_DIM],
|
|
double latitude_rad,
|
|
double longitude_rad)
|
|
{
|
|
measurement_t measurements[MAX_SATELLITES];
|
|
memcpy(measurements, clean, sizeof(measurements));
|
|
if (strcmp(scenario, "clean") == 0) {
|
|
/* No injected fault: one WLS plus integrity/covariance work. */
|
|
} else if (strcmp(scenario, "single") == 0) {
|
|
measurements[2].pseudorange_m += 100.0;
|
|
} else if (strcmp(scenario, "double") == 0) {
|
|
measurements[2].pseudorange_m += 100.0;
|
|
measurements[5].pseudorange_m += 50.0;
|
|
} else {
|
|
fprintf(stderr, "unknown benchmark scenario: %s\n", scenario);
|
|
return 2;
|
|
}
|
|
|
|
solution_t selected;
|
|
solution_t full;
|
|
for (unsigned warmup = 0; warmup < 1000; warmup++) {
|
|
if (!solve_with_fde(measurements, satellite_count, seed,
|
|
latitude_rad, longitude_rad, &selected, &full)) {
|
|
return 3;
|
|
}
|
|
benchmark_sink += selected.quality_score * 1.0e-12;
|
|
}
|
|
|
|
double start = monotonic_seconds();
|
|
for (unsigned iteration = 0; iteration < iterations; iteration++) {
|
|
/* Prevent whole-loop constant folding while keeping the scenario's
|
|
* integrity classification unchanged. */
|
|
measurements[0].pseudorange_m += (iteration & 1u) ? 1.0e-3 : -1.0e-3;
|
|
if (!solve_with_fde(measurements, satellite_count, seed,
|
|
latitude_rad, longitude_rad, &selected, &full)) {
|
|
return 3;
|
|
}
|
|
benchmark_sink += selected.quality_score * 1.0e-12;
|
|
}
|
|
double elapsed = monotonic_seconds() - start;
|
|
double nanoseconds_per_solution = elapsed * 1.0e9 / (double)iterations;
|
|
printf("benchmark scenario=%s iterations=%u elapsed=%.6f s "
|
|
"per_nav=%.1f ns (%.3f us) cpu@1Hz=%.6f%% cpu@10Hz=%.6f%% sink=%.9f\n",
|
|
scenario, iterations, elapsed, nanoseconds_per_solution,
|
|
nanoseconds_per_solution / 1000.0,
|
|
nanoseconds_per_solution / 1.0e7,
|
|
nanoseconds_per_solution / 1.0e6,
|
|
benchmark_sink);
|
|
return 0;
|
|
}
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
const unsigned satellite_count = 7;
|
|
const double latitude_rad = 39.925 * PI / 180.0;
|
|
const double longitude_rad = 32.837 * PI / 180.0;
|
|
const double receiver_height_m = 890.0;
|
|
const double clock_bias_m = 74000.0;
|
|
double receiver_truth[3];
|
|
geodetic_to_ecef(latitude_rad, longitude_rad, receiver_height_m, receiver_truth);
|
|
double seed[STATE_DIM] = {
|
|
receiver_truth[0] + 120.0,
|
|
receiver_truth[1] - 90.0,
|
|
receiver_truth[2] + 60.0,
|
|
clock_bias_m - 200.0
|
|
};
|
|
const double clean_noise[MAX_SATELLITES] =
|
|
{1.2, -1.8, 0.6, 2.0, -1.0, 1.5, -2.2, 0.0, 0.0};
|
|
measurement_t clean[MAX_SATELLITES];
|
|
create_measurements(clean, satellite_count, receiver_truth,
|
|
latitude_rad, longitude_rad, clock_bias_m, clean_noise);
|
|
|
|
if (argc >= 2 && strncmp(argv[1], "--bench-", 8) == 0) {
|
|
unsigned iterations = (argc >= 3) ? (unsigned)strtoul(argv[2], NULL, 10) : 100000u;
|
|
if (iterations == 0u) {
|
|
iterations = 1u;
|
|
}
|
|
return run_benchmark(argv[1] + 8, iterations, clean, satellite_count,
|
|
seed, latitude_rad, longitude_rad);
|
|
}
|
|
|
|
puts("Synthetic standalone GPS integrity/FDE comparison");
|
|
puts("quality = 100/(1 + chi_square/threshold); 50 is the 99% reject boundary\n");
|
|
|
|
const double receiver_samples_per_second = 16368000.0;
|
|
int anchored_week = 0;
|
|
double anchored_tow = 0.0;
|
|
double anchor_age = 0.0;
|
|
CHECK(gps_time_from_sample_anchor(2300, 46386.0,
|
|
100000000u, 99999999.75,
|
|
100000000u, 99999999.75,
|
|
receiver_samples_per_second, 12.0,
|
|
&anchored_week, &anchored_tow, &anchor_age) &&
|
|
anchored_week == 2300 && fabs(anchored_tow - 46386.0) < 1.0e-9,
|
|
"simultaneous HOW/nav-solve event must have zero elapsed time");
|
|
CHECK(gps_time_from_sample_anchor(2300, 46386.0,
|
|
100000000u, 99999999.75,
|
|
103273600u, 103273599.75,
|
|
receiver_samples_per_second, 12.0,
|
|
&anchored_week, &anchored_tow, &anchor_age),
|
|
"200 ms sample-anchor conversion should succeed");
|
|
CHECK(anchored_week == 2300 && fabs(anchored_tow - 46386.2) < 1.0e-9,
|
|
"200 ms event must be derived from receiver samples, not NAV timer ownership");
|
|
|
|
/* A channel enabled later receives a newer absolute HOW/sample anchor.
|
|
* Both channels must still calculate the same time for a common epoch. */
|
|
int late_week = 0;
|
|
double late_tow = 0.0;
|
|
CHECK(gps_time_from_sample_anchor(2300, 46392.0,
|
|
198208000u, 198207999.5,
|
|
214576000u, 214575999.5,
|
|
receiver_samples_per_second, 12.0,
|
|
&late_week, &late_tow, NULL),
|
|
"late-enable channel anchor conversion should succeed");
|
|
int early_week = 0;
|
|
double early_tow = 0.0;
|
|
CHECK(gps_time_from_sample_anchor(2300, 46386.0,
|
|
100000000u, 99999999.5,
|
|
214576000u, 214575999.5,
|
|
receiver_samples_per_second, 12.0,
|
|
&early_week, &early_tow, NULL),
|
|
"early channel anchor conversion should succeed");
|
|
CHECK(early_week == late_week && fabs(early_tow - late_tow) < 1.0e-9,
|
|
"late-enable and early channels must agree at a common receiver sample");
|
|
|
|
CHECK(gps_time_from_sample_anchor(2300, 100.0,
|
|
500000000u, 499999999.75,
|
|
549104000u, 549103999.75,
|
|
receiver_samples_per_second, 12.0,
|
|
&anchored_week, &anchored_tow, &anchor_age) &&
|
|
fabs(anchored_tow - 103.0) < 1.0e-9,
|
|
"coalesced/missed NAV events must not lose elapsed GPS time");
|
|
|
|
uint32_t wrap_anchor = UINT32_MAX - 8183999u;
|
|
uint32_t wrap_event = wrap_anchor + 16368000u;
|
|
CHECK(gps_time_from_sample_anchor(2300, 200.0,
|
|
wrap_anchor, (double)wrap_anchor - 0.25,
|
|
wrap_event, (double)wrap_event - 0.25,
|
|
receiver_samples_per_second, 12.0,
|
|
&anchored_week, &anchored_tow, NULL) &&
|
|
fabs(anchored_tow - 201.0) < 1.0e-9,
|
|
"32-bit ABS_SAMPLE rollover must unwrap correctly");
|
|
|
|
CHECK(gps_time_from_sample_anchor(2300, 604794.0,
|
|
700000000u, 699999999.5,
|
|
814576000u, 814575999.5,
|
|
receiver_samples_per_second, 12.0,
|
|
&anchored_week, &anchored_tow, NULL) &&
|
|
anchored_week == 2301 && fabs(anchored_tow - 1.0) < 1.0e-9,
|
|
"GPS week rollover must increment the anchor week");
|
|
CHECK(gps_time_resolve_how_week(true, 2300, 604794.0, 2300, 0.0) == 2301,
|
|
"HOW seconds-of-week rollover must resolve to the next week");
|
|
CHECK(!gps_time_from_sample_anchor(2300, 100.0,
|
|
100000000u, 99999999.5,
|
|
312784001u, 312784000.5,
|
|
receiver_samples_per_second, 12.0,
|
|
&anchored_week, &anchored_tow, NULL),
|
|
"stale HOW anchor must be rejected");
|
|
|
|
test_tow_t mismatched_tow[MAX_SATELLITES] = {
|
|
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8},
|
|
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.6},
|
|
{2300, 46386.8}
|
|
};
|
|
CHECK(!test_validate_tow(mismatched_tow, 7u),
|
|
"200 ms TOW mismatch must reject instead of copying the majority TOW");
|
|
test_tow_t coherent_tow[MAX_SATELLITES] = {
|
|
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8},
|
|
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8},
|
|
{2300, 46386.8}
|
|
};
|
|
CHECK(test_validate_tow(coherent_tow, 7u),
|
|
"fully coherent TOW snapshot should pass");
|
|
test_tow_t ambiguous_tow[MAX_SATELLITES] = {
|
|
{2300, 100.0}, {2300, 100.0}, {2300, 101.0}, {2300, 101.0}
|
|
};
|
|
CHECK(!test_validate_tow(ambiguous_tow, 4u),
|
|
"two-versus-two TOW split must reject the navigation epoch");
|
|
test_tow_t excessive_tow[MAX_SATELLITES] = {
|
|
{2300, 100.0}, {2300, 100.0}, {2300, 100.0}, {2300, 96.0}
|
|
};
|
|
CHECK(!test_validate_tow(excessive_tow, 4u),
|
|
"large TOW disagreement must not be silently corrected");
|
|
|
|
solution_t absolute_solution;
|
|
solution_t normalized_solution;
|
|
uint32_t full_mask = (1u << satellite_count) - 1u;
|
|
CHECK(solve_subset(clean, satellite_count, full_mask, SOLVE_ABSOLUTE_WEIGHTS,
|
|
seed, latitude_rad, longitude_rad, &absolute_solution),
|
|
"absolute-weight clean solve");
|
|
CHECK(solve_subset(clean, satellite_count, full_mask, SOLVE_NORMALIZED_WEIGHTS,
|
|
seed, latitude_rad, longitude_rad, &normalized_solution),
|
|
"normalized-weight clean solve");
|
|
print_solution("clean absolute", &absolute_solution, receiver_truth);
|
|
print_solution("clean normalized", &normalized_solution, receiver_truth);
|
|
double normalization_state_difference[3] = {
|
|
absolute_solution.state[0] - normalized_solution.state[0],
|
|
absolute_solution.state[1] - normalized_solution.state[1],
|
|
absolute_solution.state[2] - normalized_solution.state[2]
|
|
};
|
|
CHECK(vector_norm3(normalization_state_difference) < 1.0e-5,
|
|
"common weight normalization must not change WLS position");
|
|
CHECK(normalized_solution.chi_square_pass, "clean epoch should pass chi-square");
|
|
|
|
measurement_t single_fault[MAX_SATELLITES];
|
|
memcpy(single_fault, clean, sizeof(single_fault));
|
|
single_fault[2].pseudorange_m += 100.0;
|
|
solution_t single_full;
|
|
solution_t single_selected;
|
|
CHECK(solve_with_fde(single_fault, satellite_count, seed, latitude_rad, longitude_rad,
|
|
&single_selected, &single_full), "single-fault FDE solve");
|
|
print_solution("single fault: full", &single_full, receiver_truth);
|
|
print_solution("single fault: selected", &single_selected, receiver_truth);
|
|
CHECK(!single_full.chi_square_pass, "100 m fault should fail the global test");
|
|
CHECK(single_selected.chi_square_pass, "leave-one-out should recover single fault");
|
|
CHECK(single_selected.excluded_mask == (1u << 2), "FDE should exclude PRN 3 row");
|
|
CHECK(position_error_m(&single_selected, receiver_truth) <
|
|
position_error_m(&single_full, receiver_truth),
|
|
"single-fault FDE should improve position");
|
|
|
|
/* With six available satellites, exclusion leaves five measurements and
|
|
* only one residual DOF. This deterministic nominal-noise epoch is a
|
|
* regression for the large exclusion-switch jumps seen on hardware. */
|
|
const unsigned six_satellite_count = 6u;
|
|
const double ambiguous_six_noise[MAX_SATELLITES] =
|
|
{6.604, -11.586, 10.801, -5.068, 4.625, -22.908, 0.0, 0.0, 0.0};
|
|
measurement_t ambiguous_six[MAX_SATELLITES];
|
|
create_measurements(ambiguous_six, six_satellite_count, receiver_truth,
|
|
latitude_rad, longitude_rad, clock_bias_m,
|
|
ambiguous_six_noise);
|
|
solution_t unsafe_six_selected;
|
|
solution_t unsafe_six_full;
|
|
solution_t guarded_six_selected;
|
|
solution_t guarded_six_full;
|
|
CHECK(solve_with_fde_minimum(ambiguous_six, six_satellite_count, 6u, seed,
|
|
latitude_rad, longitude_rad,
|
|
&unsafe_six_selected, &unsafe_six_full),
|
|
"six-satellite legacy leave-one-out regression solve");
|
|
CHECK(solve_with_fde(ambiguous_six, six_satellite_count, seed,
|
|
latitude_rad, longitude_rad,
|
|
&guarded_six_selected, &guarded_six_full),
|
|
"six-satellite guarded FDE solve");
|
|
print_solution("six sat: full", &unsafe_six_full, receiver_truth);
|
|
print_solution("six sat: unsafe FDE", &unsafe_six_selected, receiver_truth);
|
|
print_solution("six sat: guarded", &guarded_six_selected, receiver_truth);
|
|
CHECK(!unsafe_six_full.chi_square_pass,
|
|
"six-satellite regression epoch should fail the full global test");
|
|
CHECK(unsafe_six_selected.fde_applied && unsafe_six_selected.chi_square_pass,
|
|
"unsafe six-satellite leave-one-out should demonstrate a false isolation");
|
|
CHECK(position_error_m(&unsafe_six_selected, receiver_truth) >
|
|
position_error_m(&unsafe_six_full, receiver_truth) + 50.0,
|
|
"unsafe six-satellite FDE should demonstrate the observed position regression");
|
|
CHECK(!guarded_six_selected.fde_applied,
|
|
"six-satellite solution must not replace full WLS with a one-DOF subset");
|
|
CHECK(!guarded_six_selected.solution_valid,
|
|
"failed six-satellite full solution must remain explicitly invalid");
|
|
CHECK(memcmp(guarded_six_selected.state, guarded_six_full.state,
|
|
sizeof(guarded_six_selected.state)) == 0,
|
|
"guarded six-satellite output must retain the legacy full-WLS position");
|
|
|
|
measurement_t double_fault[MAX_SATELLITES];
|
|
memcpy(double_fault, clean, sizeof(double_fault));
|
|
double_fault[2].pseudorange_m += 100.0;
|
|
double_fault[5].pseudorange_m += 50.0;
|
|
solution_t double_full;
|
|
solution_t double_selected;
|
|
CHECK(solve_with_fde(double_fault, satellite_count, seed, latitude_rad, longitude_rad,
|
|
&double_selected, &double_full), "double-fault best-effort solve");
|
|
print_solution("two faults: full", &double_full, receiver_truth);
|
|
print_solution("two faults: best effort", &double_selected, receiver_truth);
|
|
CHECK(!double_full.chi_square_pass, "two-fault full solution should fail");
|
|
CHECK(!double_selected.chi_square_pass,
|
|
"one exclusion must not claim integrity with a remaining fault");
|
|
CHECK(double_selected.chi_square_ratio < double_full.chi_square_ratio,
|
|
"best-effort candidate should retain a measurable improvement");
|
|
CHECK(position_error_m(&double_selected, receiver_truth) <
|
|
position_error_m(&double_full, receiver_truth),
|
|
"chosen synthetic best-effort case should improve position without passing integrity");
|
|
CHECK(!double_selected.solution_valid,
|
|
"improved but still inconsistent solution must remain operationally invalid");
|
|
|
|
double trusted_state[STATE_DIM];
|
|
memcpy(trusted_state, normalized_solution.state, sizeof(trusted_state));
|
|
if (double_selected.solution_valid) {
|
|
memcpy(trusted_state, double_selected.state, sizeof(trusted_state));
|
|
}
|
|
CHECK(memcmp(trusted_state, normalized_solution.state, sizeof(trusted_state)) == 0,
|
|
"invalid best-effort solution must not update trusted last_solution/PPS state");
|
|
|
|
measurement_t low_elevation_fault[MAX_SATELLITES];
|
|
memcpy(low_elevation_fault, clean, sizeof(low_elevation_fault));
|
|
low_elevation_fault[6].pseudorange_m += 100.0;
|
|
solution_t weighted_low_elevation;
|
|
solution_t uniform_low_elevation;
|
|
CHECK(solve_subset(low_elevation_fault, satellite_count, full_mask,
|
|
SOLVE_NORMALIZED_WEIGHTS, seed, latitude_rad, longitude_rad,
|
|
&weighted_low_elevation), "weighted low-elevation solve");
|
|
CHECK(solve_subset(low_elevation_fault, satellite_count, full_mask,
|
|
SOLVE_UNIFORM_WEIGHTS, seed, latitude_rad, longitude_rad,
|
|
&uniform_low_elevation), "uniform low-elevation solve");
|
|
print_solution("low-el fault: weighted", &weighted_low_elevation, receiver_truth);
|
|
print_solution("low-el fault: uniform", &uniform_low_elevation, receiver_truth);
|
|
CHECK(position_error_m(&weighted_low_elevation, receiver_truth) <
|
|
position_error_m(&uniform_low_elevation, receiver_truth),
|
|
"elevation weighting should reduce a low-elevation fault's position effect");
|
|
|
|
CHECK(normalized_solution.covariance_enu_m2[0][0] > 0.0 &&
|
|
normalized_solution.covariance_enu_m2[1][1] > 0.0 &&
|
|
normalized_solution.covariance_enu_m2[2][2] > 0.0,
|
|
"ENU covariance diagonal must be positive");
|
|
|
|
if (failures == 0) {
|
|
puts("\nPASS: all synthetic integrity checks passed");
|
|
return 0;
|
|
}
|
|
fprintf(stderr, "\n%d synthetic integrity check(s) failed\n", failures);
|
|
return 1;
|
|
}
|