fix(gps): adapt Doppler bandwidth from board validation

This commit is contained in:
2026-07-26 03:15:52 +03:00
parent 23650be635
commit 156cfc8b77
24 changed files with 592 additions and 49 deletions
@@ -138,6 +138,99 @@ static void test_bias_ramp_without_doppler_uses_bounded_fallback(void)
assert(sqrt(squared_error_sum / (double)tracked) < 45.0);
}
static void test_board_calibrated_doppler_predictor_beats_fallback(void)
{
/* Values measured on Genesys2 on 2026-07-26: clock-bias sigma was
* 15.87 m and reference-datum drift was about 291.75 m/s. Feed the
* exact same deterministic WLS noise to two filters so this is a true
* predictor on/off A/B, not two unrelated Monte-Carlo runs. */
static const double bias_noise_m[] = {
8.0, -18.0, 12.0, -7.0, 21.0, -14.0, 5.0, -25.0,
16.0, -3.0, 10.0, -20.0, 6.0, 24.0, -11.0, 2.0
};
static const double drift_noise_mps[] = {
1.8, -3.2, 4.1, -2.4, 0.7, 3.5, -4.3, 2.6,
-1.1, 4.6, -2.8, 1.5, -3.7, 2.2, -0.4, 3.0
};
gps_pps_time_filter_t aided;
gps_pps_time_filter_t fallback;
gps_pps_time_filter_init(&aided);
gps_pps_time_filter_init(&fallback);
double aided_squared_error_sum = 0.0;
double fallback_squared_error_sum = 0.0;
unsigned int tracked = 0u;
for(unsigned int update = 0u; update < 900u; update++)
{
double time_sec = 0.2 * (double)update;
double long_omega = 2.0 * 3.14159265358979323846 / 75.0;
double short_omega = 2.0 * 3.14159265358979323846 / 19.0;
double varying_bias_m =
5.0 * (1.0 - cos(long_omega * time_sec)) / long_omega +
2.0 * (1.0 - cos(short_omega * time_sec)) / short_omega;
double true_drift_mps =
291.75 + 5.0 * sin(long_omega * time_sec) +
2.0 * sin(short_omega * time_sec);
double true_bias_m =
159624.654 + 291.75 * time_sec + varying_bias_m;
double measured_bias_m = true_bias_m +
bias_noise_m[update %
(sizeof(bias_noise_m) / sizeof(bias_noise_m[0]))];
double aided_bias_m = 0.0;
double fallback_bias_m = 0.0;
double innovation_m = 0.0;
gps_pps_time_filter_result_t aided_result =
gps_pps_time_filter_update(
&aided,
(uint64_t)llround(time_sec * COUNTER_HZ),
COUNTER_HZ,
measured_bias_m,
true,
true_drift_mps + drift_noise_mps[update %
(sizeof(drift_noise_mps) /
sizeof(drift_noise_mps[0]))],
1.5,
1.0 / 32.0,
12u,
&aided_bias_m,
&innovation_m);
gps_pps_time_filter_result_t fallback_result =
gps_pps_time_filter_update(
&fallback,
(uint64_t)llround(time_sec * COUNTER_HZ),
COUNTER_HZ,
measured_bias_m,
false,
0.0,
1.5,
1.0 / 32.0,
12u,
&fallback_bias_m,
&innovation_m);
assert(aided_result != GPS_PPS_TIME_FILTER_REJECTED);
assert(fallback_result != GPS_PPS_TIME_FILTER_REJECTED);
if((update >= 300u) &&
(aided_result == GPS_PPS_TIME_FILTER_TRACKING) &&
(fallback_result == GPS_PPS_TIME_FILTER_TRACKING))
{
double aided_error_m = aided_bias_m - true_bias_m;
double fallback_error_m = fallback_bias_m - true_bias_m;
aided_squared_error_sum += aided_error_m * aided_error_m;
fallback_squared_error_sum +=
fallback_error_m * fallback_error_m;
tracked++;
}
}
assert(tracked > 550u);
double aided_rms_m = sqrt(aided_squared_error_sum / (double)tracked);
double fallback_rms_m =
sqrt(fallback_squared_error_sum / (double)tracked);
printf("board PPS bias RMS: Doppler=%.3f m fallback=%.3f m\n",
aided_rms_m, fallback_rms_m);
assert(aided_rms_m < 8.0);
assert(aided_rms_m < 0.35 * fallback_rms_m);
}
static void test_reference_geometry_defines_bias_drift(void)
{
const double receiver_position_m[3] = {0.0, 0.0, 0.0};
@@ -208,6 +301,7 @@ int main(void)
test_drift_prediction_rejects_bias_noise();
test_quality_gap_and_outlier_handling();
test_bias_ramp_without_doppler_uses_bounded_fallback();
test_board_calibrated_doppler_predictor_beats_fallback();
test_reference_geometry_defines_bias_drift();
test_reference_selection_is_stable();
puts("gps_pps_time_filter tests passed");
@@ -86,6 +86,14 @@ static void test_cn0_and_mailbox_decode(void)
measurement.filtered_pseudorange_rate_mps,
measurement.pseudorange_rate_mps,
1.0e-12));
assert(close_to(
measurement.eskf_doppler_hz,
measurement.doppler_hz,
1.0e-12));
assert(close_to(
measurement.eskf_pseudorange_rate_mps,
measurement.pseudorange_rate_mps,
1.0e-12));
registers.MEASUREMENT_META &=
~(1u << GPS_TRCK_MEAS_META_LOCK_POS);