/* * The measurement model: how much one satellite is allowed to move the fix. * * Three quantities were being conflated in this code and it is worth naming * them here, because only one of them is a bias: * * - the corrections, tropo() and klobuchar_delay_meters(), which are * subtracted from the observation to remove a real physical delay; * - the sigma, gps_nav_measurement_sigma_m(), which is not a bias at all * and never shifts anything - it only says how much to trust a satellite; * - the WLS clock bias, the fourth unknown, which is solved for. * * The defects these tests pin down were in the first two: a weighting floor * that stopped the sigma model working below 10 degrees, and corrections that * switched themselves off below the horizon and left their whole magnitude in * the measurement as an uncorrected bias. */ #include #include #include #include #include "gps/gps_ip_defines.h" #include "gps/NavEquations.h" #include "gps/matrixLib.h" static int failures; #define CHECK(condition, message) do { \ if(!(condition)) { \ fprintf(stderr, "FAIL: %s\n", message); \ failures++; \ } \ } while(0) #define URA_INDEX 2u /* 4.85 m, a typical broadcast value */ /* * The sigma model has to keep working where a satellite is worst. The floor * used to be 10 degrees, which pinned the elevation term at 17.3 m: at 5 * degrees the model's own answer is 34.4 m and at 2 degrees it is 86 m, so a * low satellite was trusted 4 to 25 times more than this same model said it * should be. Nothing was ever excluded by that floor - it only stopped the * mapping from doing its job. */ static void test_sigma_keeps_degrading_below_ten_degrees(void) { const double elevations[] = {90.0, 45.0, 30.0, 15.0, 10.0, 7.0, 5.0, 3.0}; double previous = 0.0; printf(" elevation -> sigma relative weight vs zenith\n"); double zenith = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true, NAN, false); for(unsigned int i = 0u; i < (sizeof(elevations) / sizeof(elevations[0])); i++) { double sigma = gps_nav_measurement_sigma_m( URA_INDEX, elevations[i], true, NAN, false); printf(" %5.1f deg -> %8.1f m %8.4f\n", elevations[i], sigma, (zenith * zenith) / (sigma * sigma)); CHECK(isfinite(sigma) && (sigma > 0.0), "sigma must stay finite and positive at every elevation"); if(i != 0u) { CHECK(sigma > previous, "sigma stopped growing as elevation fell"); } previous = sigma; } /* The model's own numbers, now that nothing truncates them. */ double at_ten = gps_nav_measurement_sigma_m(URA_INDEX, 10.0, true, NAN, false); double at_five = gps_nav_measurement_sigma_m(URA_INDEX, 5.0, true, NAN, false); CHECK(at_five > (1.8 * at_ten), "a five degree satellite is still weighted like a ten degree one"); printf(" 5 deg sigma %.1f m vs 10 deg %.1f m: %.1fx the variance\n", at_five, at_ten, (at_five * at_five) / (at_ten * at_ten)); } /* * The floor exists because 1/sin diverges at the horizon and a zero weight is * not a discounted satellite - least_squares_build_postfit() refuses a * non-positive weight and the whole epoch fails. So however low a satellite * goes, the sigma must stay finite, positive, and monotone. */ static void test_sigma_is_bounded_at_and_below_the_horizon(void) { const double elevations[] = {1.0, 0.5, 0.0, -1.0, -5.0, -20.0}; for(unsigned int i = 0u; i < (sizeof(elevations) / sizeof(elevations[0])); i++) { double sigma = gps_nav_measurement_sigma_m( URA_INDEX, elevations[i], true, NAN, false); CHECK(isfinite(sigma) && (sigma > 0.0), "sigma went non-finite or non-positive below the horizon"); } /* Below the horizon the correction is extrapolated and the signal has * probably arrived by reflection, so it must cost more than merely being * low. */ double just_above = gps_nav_measurement_sigma_m(URA_INDEX, 0.5, true, NAN, false); double just_below = gps_nav_measurement_sigma_m(URA_INDEX, -0.5, true, NAN, false); CHECK(just_below > just_above, "a satellite below the horizon was not charged for it"); /* A non-finite elevation is unknown, not good: it must not be treated as * a healthy satellite. */ double unknown = gps_nav_measurement_sigma_m(URA_INDEX, NAN, true, NAN, false); double at_thirty = gps_nav_measurement_sigma_m(URA_INDEX, 30.0, true, NAN, false); CHECK(unknown > at_thirty, "a non-finite elevation was treated as a good satellite"); printf(" horizon bounded: 0.5 deg %.1f m, -0.5 deg %.1f m, " "unknown %.1f m\n", just_above, just_below, unknown); } /* * DLL code-tracking jitter goes as the inverse square root of carrier-to-noise * density in linear power, so 20 dB less signal is 100 times less power and * exactly ten times the sigma. This checks the law rather than the constant. */ static void test_cn0_follows_the_tracking_law(void) { /* Isolate the C/N0 term by holding elevation at zenith. */ double base = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true, 45.0, true); double minus20 = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true, 25.0, true); double ura = 4.85; double zen = 3.0; double common = (ura * ura) + (zen * zen); double term_base = sqrt((base * base) - common); double term_minus20 = sqrt((minus20 * minus20) - common); printf(" C/N0 45 dB-Hz -> %.2f m, 25 dB-Hz -> %.2f m (ratio %.2f)\n", term_base, term_minus20, term_minus20 / term_base); CHECK(fabs(term_base - 3.0) < 1e-9, "the C/N0 term is not 3 m at 45 dB-Hz"); CHECK(fabs((term_minus20 / term_base) - 10.0) < 1e-6, "20 dB of C/N0 did not scale sigma by ten"); /* A weaker satellite is always trusted less, never more. */ double previous = 0.0; for(double cn0 = 50.0; cn0 >= 28.0; cn0 -= 2.0) { double sigma = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true, cn0, true); CHECK(sigma > previous, "sigma did not grow as C/N0 fell"); previous = sigma; } } /* * The correction discontinuity. * * tropo() used to be called only for elevation above zero and Klobuchar * returned zero at or below it, so a satellite crossing the horizon lost its * entire atmospheric correction in one step - tens of metres appearing in the * measurement as an uncorrected bias, from one epoch to the next, for no * physical reason. Both are now evaluated at the horizon instead. * * tropo() is the public half of that pair, so the continuity is checked * directly on it; the ionospheric half is exercised through the solve below. */ static void test_troposphere_is_continuous_through_the_horizon(void) { const double dtr = 0.01745329251994329576923690768489; double above = tropo(sin(0.05 * dtr), 0.9); double at = tropo(sin(0.0), 0.9); double below = tropo(sin(-0.05 * dtr), 0.9); printf(" troposphere at +0.05/0.00/-0.05 deg: %.2f / %.2f / %.2f m\n", above, at, below); CHECK(isfinite(above) && isfinite(at) && isfinite(below), "the tropospheric model went non-finite near the horizon"); /* * The model is genuinely steep here - about 30 m per degree - so this is * about continuity, not flatness. What used to happen is the whole value * dropping to zero the moment elevation went non-positive. */ CHECK(fabs(above - at) < 5.0, "the tropospheric model steps across the horizon"); CHECK(fabs(below - at) < 1.0e-9, "the tropospheric model is not clamped at the horizon"); CHECK(at > 20.0, "the horizon tropospheric delay is implausibly small; dropping it " "would leave that much uncorrected bias"); printf(" -> below the horizon it holds %.1f m instead of dropping to 0\n", below); } /* ------------------------------------------------------------------ */ /* End to end: what a biased low satellite does to the fix. */ /* ------------------------------------------------------------------ */ /* Receiver near 40 N, 33 E, on the ellipsoid. */ static const double RECEIVER[3] = {4121000.0, 2699000.0, 4037000.0}; #define TRUE_CLOCK_BIAS_M 12345.0 /* * Place a satellite at a given azimuth/elevation as seen from RECEIVER, at * GPS orbital radius, by walking out along the local line of sight. */ static void place_satellite(double azimuth_deg, double elevation_deg, double out[3]) { const double dtr = 0.01745329251994329576923690768489; double r = sqrt((RECEIVER[0] * RECEIVER[0]) + (RECEIVER[1] * RECEIVER[1]) + (RECEIVER[2] * RECEIVER[2])); double ground = sqrt((RECEIVER[0] * RECEIVER[0]) + (RECEIVER[1] * RECEIVER[1])); double sin_lat = RECEIVER[2] / r; double cos_lat = ground / r; double sin_lon = RECEIVER[1] / ground; double cos_lon = RECEIVER[0] / ground; double el = elevation_deg * dtr; double az = azimuth_deg * dtr; double up = sin(el); double north = cos(el) * cos(az); double east = cos(el) * sin(az); /* Local east/north/up back into ECEF. */ double dx = (-sin_lon * east) + (-sin_lat * cos_lon * north) + (cos_lat * cos_lon * up); double dy = (cos_lon * east) + (-sin_lat * sin_lon * north) + (cos_lat * sin_lon * up); double dz = (cos_lat * north) + (sin_lat * up); /* Range to a satellite at 26560 km geocentric radius on that bearing. */ double b = 2.0 * ((RECEIVER[0] * dx) + (RECEIVER[1] * dy) + (RECEIVER[2] * dz)); double c = (r * r) - (26560000.0 * 26560000.0); double range = (-b + sqrt((b * b) - (4.0 * c))) / 2.0; out[0] = RECEIVER[0] + (range * dx); out[1] = RECEIVER[1] + (range * dy); out[2] = RECEIVER[2] + (range * dz); } /* * Solve one epoch and report how far the fix ends up from the truth. * bias_row/bias_m inject an uncorrected error on one satellite, which is what * a satellite whose atmospheric correction was dropped actually looks like. */ static bool solve_epoch(const double azimuth[], const double elevation[], unsigned int count, int bias_row, double bias_m, double solved_ecef_m[3], double* solved_clock_m) { mat_type* satpos = create_matrix((int)count, 3, "satpos"); mat_type* obs = create_matrix(1, (int)count, "obs"); mat_type* az = create_matrix(1, (int)count, "az"); mat_type* el = create_matrix(1, (int)count, "el"); unsigned char ura[GPS_TRCK_IP_COUNT]; gps_nav_solution_t solution; bool ok = false; if((satpos == NULL) || (obs == NULL) || (az == NULL) || (el == NULL)) { goto done; } initialize_all(0.0, satpos); initialize_all(0.0, obs); initialize_all(0.0, az); initialize_all(0.0, el); for(unsigned int i = 0u; i < count; i++) { double sat[3]; place_satellite(azimuth[i], elevation[i], sat); for(unsigned int k = 0u; k < 3u; k++) { satpos->matrix[i][k] = sat[k]; } double dx = sat[0] - RECEIVER[0]; double dy = sat[1] - RECEIVER[1]; double dz = sat[2] - RECEIVER[2]; double range = sqrt((dx * dx) + (dy * dy) + (dz * dz)); obs->matrix[0][i] = range + TRUE_CLOCK_BIAS_M; if((bias_row >= 0) && ((unsigned int)bias_row == i)) { obs->matrix[0][i] += bias_m; } ura[i] = URA_INDEX; } /* No ionosphere model and no C/N0, so the only thing separating these * satellites is geometry and the elevation weighting under test. */ if(!leastSquaresSTD(satpos, obs, (int)count, az, el, ura, NULL, NULL, 300000.0, &solution)) { goto done; } for(unsigned int k = 0u; k < 3u; k++) { solved_ecef_m[k] = solution.ecef_m[k]; } *solved_clock_m = solution.clock_bias_m; ok = true; done: free_matrix(satpos); free_matrix(obs); free_matrix(az); free_matrix(el); return ok; } /* * Six satellites, one of them at 4 degrees. Clean, the solve must recover the * truth; with 30 m of uncorrected delay on the low one, the fix must move by * much less than that, because the weighting now knows how little that * satellite is worth. * * The 30 m is not arbitrary: it is roughly the tropospheric delay that used to * be dropped outright the moment a satellite crossed the horizon. */ static double displacement(const double a[3], const double b[3]) { double dx = a[0] - b[0]; double dy = a[1] - b[1]; double dz = a[2] - b[2]; return sqrt((dx * dx) + (dy * dy) + (dz * dz)); } /* * Six satellites, one of them at 4 degrees. Inject 30 m of uncorrected delay * on one of them and measure how far the fix moves - the displacement between * two solves, not the distance from truth, because the synthetic observations * deliberately carry no atmosphere and the model's own correction is a common * offset that says nothing about the question being asked. * * The 30 m is not arbitrary: it is roughly the tropospheric delay that used to * be dropped outright the moment a satellite crossed the horizon. * * The property under test is that the solve is *less* sensitive to a bias on a * satellite it should not trust than to the same bias on one it should. With * the elevation term floored at 10 degrees the two were nearly equal, which is * what made a setting satellite as damaging as an overhead one. */ static void test_a_biased_low_satellite_moves_the_fix_less_than_its_bias(void) { const double azimuth[6] = { 10.0, 75.0, 140.0, 205.0, 280.0, 330.0}; const double elevation[6] = { 72.0, 41.0, 28.0, 55.0, 33.0, 4.0}; double clean[3]; double clean_clock = 0.0; if(!solve_epoch(azimuth, elevation, 6u, -1, 0.0, clean, &clean_clock)) { fprintf(stderr, "FAIL: the clean epoch did not solve\n"); failures++; return; } double low[3]; double low_clock = 0.0; double high[3]; double high_clock = 0.0; if(!solve_epoch(azimuth, elevation, 6u, 5, 30.0, low, &low_clock) || !solve_epoch(azimuth, elevation, 6u, 0, 30.0, high, &high_clock)) { fprintf(stderr, "FAIL: a biased epoch did not solve\n"); failures++; return; } double low_move = displacement(low, clean); double high_move = displacement(high, clean); printf(" 30 m of uncorrected delay moves the fix by %.2f m on the " "4 deg satellite, %.2f m on the 72 deg one\n", low_move, high_move); printf(" and the clock bias by %.2f m / %.2f m\n", fabs(low_clock - clean_clock), fabs(high_clock - clean_clock)); /* * What the ten degree floor did to that same satellite: it pinned the * elevation term at 3/sin(10 deg) instead of 3/sin(4 deg), so the * satellite entered the normal equations with several times the weight * this model says it has earned - and the fix moved proportionally more. */ { const double dtr = 0.01745329251994329576923690768489; double ura = 4.85; double floored = sqrt((ura * ura) + pow(3.0 / sin(10.0 * dtr), 2.0)); double honest = gps_nav_measurement_sigma_m(URA_INDEX, 4.0, true, NAN, false); printf(" the 10 deg floor gave that satellite sigma %.1f m instead " "of %.1f m: %.1fx the weight\n", floored, honest, (honest * honest) / (floored * floored)); CHECK(honest > floored, "the floor being regressed against no longer reproduces"); } CHECK(low_move > 0.0, "the biased solve did not move at all"); CHECK(low_move < high_move, "a bias on the lowest satellite pulled the fix at least as hard as " "the same bias on the highest one"); CHECK(low_move < 30.0, "a 30 m bias on the lowest satellite moved the fix by more than the " "bias itself"); } int main(void) { test_sigma_keeps_degrading_below_ten_degrees(); test_sigma_is_bounded_at_and_below_the_horizon(); test_cn0_follows_the_tracking_law(); test_troposphere_is_continuous_through_the_horizon(); test_a_biased_low_satellite_moves_the_fix_less_than_its_bias(); if(failures != 0) { fprintf(stderr, "%d check(s) failed\n", failures); return 1; } printf("gps measurement model tests passed\n"); return 0; }