fix(gps): adapt Doppler bandwidth from board validation
This commit is contained in:
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user