463 lines
17 KiB
Python
463 lines
17 KiB
Python
from __future__ import annotations
|
|
|
|
import math
|
|
import random
|
|
import unittest
|
|
|
|
from pps_python.adaptive_pps import (
|
|
ACQUIRE_PROFILE,
|
|
FixedPointAdaptivePps,
|
|
Observation,
|
|
Profile,
|
|
PROFILES,
|
|
QualityMetrics,
|
|
QualitySelector,
|
|
run_observations,
|
|
)
|
|
|
|
|
|
def quality(profile: Profile) -> QualityMetrics:
|
|
if profile == Profile.PRECISION:
|
|
return QualityMetrics(
|
|
1.7, 3.2, 18.0, 38.0, 27.0, 0.35, 0.25, 7
|
|
)
|
|
if profile == Profile.ROBUST:
|
|
return QualityMetrics(
|
|
5.0, 8.0, 65.0, 27.0, 18.0, 0.85, 1.8, 5
|
|
)
|
|
return QualityMetrics(
|
|
12.0, 18.0, 180.0, 18.0, 10.0, 2.0, 4.0, 4
|
|
)
|
|
|
|
|
|
def noisy_observations(
|
|
duration_s: float,
|
|
*,
|
|
seed: int,
|
|
missing: tuple[float, float] | None = None,
|
|
quality_profile: Profile = Profile.PRECISION,
|
|
):
|
|
rng = random.Random(seed)
|
|
observations = []
|
|
update = 0
|
|
while update * 0.2 < duration_s:
|
|
time_s = update * 0.2
|
|
update += 1
|
|
if missing is not None and missing[0] <= time_s < missing[1]:
|
|
continue
|
|
noise_s = rng.gauss(0.0, 320.0e-9)
|
|
if update % 37 == 0:
|
|
noise_s += rng.choice((-1.0, 1.0)) * 900.0e-9
|
|
observations.append(
|
|
Observation(
|
|
time_s,
|
|
noise_s,
|
|
rng.gauss(0.0, 0.25),
|
|
quality(quality_profile),
|
|
)
|
|
)
|
|
return observations
|
|
|
|
|
|
class AdaptivePpsTests(unittest.TestCase):
|
|
def test_period_aware_coefficients_match_firmware(self):
|
|
expected = {
|
|
0.2: {
|
|
"acquire": (764018, 64006),
|
|
Profile.PRECISION: (222838, 5445),
|
|
Profile.ROBUST: (84891, 640),
|
|
},
|
|
0.5: {
|
|
"acquire": (615780, 127492),
|
|
Profile.PRECISION: (209783, 12804),
|
|
Profile.ROBUST: (82980, 1564),
|
|
},
|
|
1.0: {
|
|
"acquire": (447388, 178038),
|
|
Profile.PRECISION: (190239, 23149),
|
|
Profile.ROBUST: (79923, 3011),
|
|
},
|
|
}
|
|
for period_s, coefficients in expected.items():
|
|
self.assertEqual(
|
|
ACQUIRE_PROFILE.fixed_coefficients(period_s)[:2],
|
|
coefficients["acquire"],
|
|
)
|
|
for profile in (Profile.PRECISION, Profile.ROBUST):
|
|
self.assertEqual(
|
|
PROFILES[profile].fixed_coefficients(period_s)[:2],
|
|
coefficients[profile],
|
|
)
|
|
|
|
def test_wall_clock_hysteresis_and_settling_scale_with_period(self):
|
|
model_500ms = FixedPointAdaptivePps(update_period_s=0.5)
|
|
self.assertEqual(model_500ms.selector.upgrade_updates, 4)
|
|
self.assertEqual(model_500ms.selector.degrade_updates, 2)
|
|
self.assertEqual(
|
|
model_500ms.profiles[Profile.PRECISION].software_settle_updates,
|
|
5,
|
|
)
|
|
self.assertEqual(
|
|
model_500ms.profiles[Profile.ROBUST].lock_acquire_updates,
|
|
4,
|
|
)
|
|
|
|
model_1s = FixedPointAdaptivePps(update_period_s=1.0)
|
|
self.assertEqual(model_1s.selector.upgrade_updates, 2)
|
|
self.assertEqual(model_1s.selector.degrade_updates, 2)
|
|
self.assertEqual(
|
|
model_1s.profiles[Profile.PRECISION].software_settle_updates,
|
|
3,
|
|
)
|
|
self.assertEqual(
|
|
model_1s.profiles[Profile.ROBUST].lock_acquire_updates,
|
|
2,
|
|
)
|
|
self.assertEqual(model_1s.reference_watchdog_sec, 3.0)
|
|
|
|
def test_board_calibrated_doppler_predictor_reduces_pps_error(self):
|
|
"""Genesys2 WLS statistics: predictor-on must beat fallback-only."""
|
|
quality_metrics = QualityMetrics(
|
|
1.50, 2.92, 15.87, 40.3, 34.8, 0.35, 0.23, 7
|
|
)
|
|
aided_phase_rms = []
|
|
fallback_phase_rms = []
|
|
aided_edge_jitter = []
|
|
fallback_edge_jitter = []
|
|
for seed in range(8):
|
|
rng = random.Random(7300 + seed)
|
|
observations = []
|
|
for update in range(900):
|
|
time_s = 0.2 * update
|
|
long_omega = 2.0 * math.pi / 75.0
|
|
short_omega = 2.0 * math.pi / 19.0
|
|
varying_drift_mps = (
|
|
5.0 * math.sin(long_omega * time_s)
|
|
+ 2.0 * math.sin(short_omega * time_s)
|
|
)
|
|
varying_bias_m = (
|
|
5.0 * (1.0 - math.cos(long_omega * time_s)) / long_omega
|
|
+ 2.0 * (1.0 - math.cos(short_omega * time_s))
|
|
/ short_omega
|
|
)
|
|
# 15.87 m / c = 52.94 ns; 3.47 m/s is the measured
|
|
# board standard deviation of the reference bias drift.
|
|
noise_s = rng.gauss(0.0, 15.87 / 299_792_458.0)
|
|
if update % 113 == 0 and update != 0:
|
|
noise_s += rng.choice((-1.0, 1.0)) * 120.0e-9
|
|
observations.append(
|
|
Observation(
|
|
time_s,
|
|
noise_s,
|
|
varying_drift_mps + rng.gauss(0.0, 3.47),
|
|
quality_metrics,
|
|
True,
|
|
-291.75,
|
|
1,
|
|
varying_bias_m,
|
|
)
|
|
)
|
|
aided = run_observations(
|
|
observations,
|
|
finish_time_s=180.0,
|
|
oscillator_ppm=1.592,
|
|
)
|
|
fallback = run_observations(
|
|
[
|
|
Observation(
|
|
item.time_s,
|
|
item.timing_error_s,
|
|
item.clock_drift_error_mps,
|
|
item.quality,
|
|
False,
|
|
item.reference_range_rate_mps,
|
|
item.reference_id,
|
|
item.reference_datum_offset_m,
|
|
)
|
|
for item in observations
|
|
],
|
|
finish_time_s=180.0,
|
|
oscillator_ppm=1.592,
|
|
)
|
|
aided_phase_rms.append(aided.phase_rms(after_s=60.0))
|
|
fallback_phase_rms.append(fallback.phase_rms(after_s=60.0))
|
|
aided_edge_jitter.append(
|
|
aided.one_second_edge_jitter_rms(after_s=60.0)
|
|
)
|
|
fallback_edge_jitter.append(
|
|
fallback.one_second_edge_jitter_rms(after_s=60.0)
|
|
)
|
|
self.assertEqual(aided.unlock_events, 0)
|
|
|
|
self.assertLess(
|
|
sum(aided_phase_rms) / len(aided_phase_rms),
|
|
0.35 * sum(fallback_phase_rms) / len(fallback_phase_rms),
|
|
)
|
|
self.assertLess(
|
|
sum(aided_edge_jitter) / len(aided_edge_jitter),
|
|
0.70 * sum(fallback_edge_jitter) / len(fallback_edge_jitter),
|
|
)
|
|
self.assertLess(max(aided_phase_rms), 30.0e-9)
|
|
|
|
def test_quality_hysteresis_rejects_metric_chatter(self):
|
|
selector = QualitySelector()
|
|
for _ in range(7):
|
|
self.assertEqual(selector.update(quality(Profile.PRECISION))[0],
|
|
Profile.HOLDOVER)
|
|
self.assertEqual(selector.update(quality(Profile.PRECISION))[0],
|
|
Profile.PRECISION)
|
|
|
|
# A one-epoch DOP/CN0 jump must not switch coefficients.
|
|
self.assertEqual(selector.update(quality(Profile.ROBUST))[0],
|
|
Profile.PRECISION)
|
|
self.assertEqual(selector.update(quality(Profile.PRECISION))[0],
|
|
Profile.PRECISION)
|
|
|
|
for _ in range(2):
|
|
self.assertEqual(selector.update(quality(Profile.ROBUST))[0],
|
|
Profile.PRECISION)
|
|
self.assertEqual(selector.update(quality(Profile.ROBUST))[0],
|
|
Profile.ROBUST)
|
|
|
|
invalid = quality(Profile.PRECISION)
|
|
invalid = QualityMetrics(**{
|
|
**invalid.__dict__, "solution_valid": False
|
|
})
|
|
for _ in range(2):
|
|
self.assertEqual(selector.update(invalid)[0], Profile.ROBUST)
|
|
self.assertEqual(selector.update(invalid)[0], Profile.HOLDOVER)
|
|
|
|
def test_unfiltered_board_noise_does_not_meet_lock_contract(self):
|
|
observations = noisy_observations(90.0, seed=17)
|
|
trace = run_observations(
|
|
observations,
|
|
finish_time_s=90.0,
|
|
adaptive=False,
|
|
software_filter=False,
|
|
)
|
|
self.assertLess(trace.locked_fraction(), 0.25)
|
|
self.assertIsNone(trace.first_lock_time())
|
|
|
|
def test_adaptive_filters_lock_noisy_wls(self):
|
|
observations = noisy_observations(120.0, seed=17)
|
|
trace = run_observations(observations, finish_time_s=120.0)
|
|
first_lock = trace.first_lock_time()
|
|
self.assertIsNotNone(first_lock)
|
|
self.assertLess(first_lock, 20.0)
|
|
tail = trace.phase_error_s[len(trace.phase_error_s) // 2 :]
|
|
rms = math.sqrt(sum(value * value for value in tail) / len(tail))
|
|
self.assertLess(rms, 120.0e-9)
|
|
self.assertLess(
|
|
trace.one_second_jitter_rms(after_s=60.0),
|
|
20.0e-9,
|
|
)
|
|
self.assertGreater(trace.locked_fraction(), 0.80)
|
|
self.assertLessEqual(trace.unlock_events, 1)
|
|
|
|
def test_bounded_wls_gap_holds_then_resumes_without_coarse_step(self):
|
|
observations = noisy_observations(
|
|
100.0, seed=23, missing=(35.0, 50.0)
|
|
)
|
|
trace = run_observations(observations, finish_time_s=100.0)
|
|
baseline = run_observations(
|
|
noisy_observations(100.0, seed=23),
|
|
finish_time_s=100.0,
|
|
)
|
|
before_gap = [
|
|
locked for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if 30.0 <= time_s < 35.0
|
|
]
|
|
self.assertTrue(before_gap and all(before_gap))
|
|
# The two-second watchdog suppresses PPS validity during the gap, but
|
|
# a bounded outage with the same datum retains the Doppler/PI states.
|
|
# Recovery therefore needs no extra coarse commit or phase step.
|
|
just_after_gap = [
|
|
locked for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if 50.0 <= time_s < 51.0
|
|
]
|
|
self.assertTrue(just_after_gap and all(just_after_gap))
|
|
late = [
|
|
locked for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if time_s >= 70.0
|
|
]
|
|
self.assertTrue(late and sum(late) / len(late) > 0.9)
|
|
self.assertEqual(trace.coarse_commits, baseline.coarse_commits)
|
|
|
|
def test_very_long_wls_gap_reanchors_coarsely(self):
|
|
observations = noisy_observations(
|
|
130.0, seed=29, missing=(35.0, 70.0)
|
|
)
|
|
trace = run_observations(observations, finish_time_s=130.0)
|
|
after_gap = [
|
|
locked for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if 70.0 <= time_s < 75.0
|
|
]
|
|
late = [
|
|
locked for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if time_s >= 90.0
|
|
]
|
|
self.assertTrue(after_gap and not any(after_gap))
|
|
self.assertTrue(late and sum(late) / len(late) > 0.9)
|
|
self.assertGreaterEqual(trace.coarse_commits, 2)
|
|
|
|
def test_no_wls_never_claims_lock(self):
|
|
trace = run_observations([], finish_time_s=60.0)
|
|
self.assertIsNone(trace.first_lock_time())
|
|
self.assertEqual(trace.lock_events, 0)
|
|
|
|
def test_satellite_loss_invalidates_then_resumes_predicted_state(self):
|
|
observations = noisy_observations(110.0, seed=41)
|
|
invalid_quality = QualityMetrics(
|
|
**{
|
|
**quality(Profile.PRECISION).__dict__,
|
|
"solution_valid": False,
|
|
"used_satellites": 2,
|
|
"velocity_valid": False,
|
|
}
|
|
)
|
|
observations = [
|
|
Observation(
|
|
item.time_s,
|
|
item.timing_error_s,
|
|
item.clock_drift_error_mps,
|
|
(
|
|
invalid_quality
|
|
if 40.0 <= item.time_s < 55.0
|
|
else item.quality
|
|
),
|
|
item.clock_drift_valid,
|
|
)
|
|
for item in observations
|
|
]
|
|
trace = run_observations(observations, finish_time_s=110.0)
|
|
baseline = run_observations(
|
|
noisy_observations(110.0, seed=41),
|
|
finish_time_s=110.0,
|
|
)
|
|
during_loss = [
|
|
valid
|
|
for time_s, valid in zip(trace.time_s, trace.locked)
|
|
if 42.2 <= time_s < 55.0
|
|
]
|
|
after_recovery = [
|
|
valid
|
|
for time_s, valid in zip(trace.time_s, trace.locked)
|
|
if time_s >= 75.0
|
|
]
|
|
self.assertTrue(during_loss and not any(during_loss))
|
|
self.assertTrue(after_recovery and sum(after_recovery) / len(after_recovery) > 0.9)
|
|
self.assertEqual(trace.coarse_commits, baseline.coarse_commits)
|
|
|
|
def test_wls_bias_drift_is_negative_reference_range_rate(self):
|
|
observations = [
|
|
Observation(
|
|
item.time_s,
|
|
item.timing_error_s,
|
|
item.clock_drift_error_mps,
|
|
item.quality,
|
|
item.clock_drift_valid,
|
|
350.0,
|
|
)
|
|
for item in noisy_observations(120.0, seed=53)
|
|
]
|
|
trace = run_observations(observations, finish_time_s=120.0)
|
|
self.assertIsNotNone(trace.first_lock_time())
|
|
self.assertLess(trace.first_lock_time(), 20.0)
|
|
self.assertLess(
|
|
trace.one_second_jitter_rms(after_s=60.0),
|
|
20.0e-9,
|
|
)
|
|
|
|
def test_reference_switch_reanchors_instead_of_rejecting_datum_jump(self):
|
|
observations = [
|
|
Observation(
|
|
item.time_s,
|
|
item.timing_error_s,
|
|
item.clock_drift_error_mps,
|
|
item.quality,
|
|
item.clock_drift_valid,
|
|
180.0 if item.time_s < 50.0 else -90.0,
|
|
3 if item.time_s < 50.0 else 8,
|
|
0.0 if item.time_s < 50.0 else 38000.0,
|
|
)
|
|
for item in noisy_observations(120.0, seed=59)
|
|
]
|
|
trace = run_observations(observations, finish_time_s=120.0)
|
|
just_after_switch = [
|
|
locked
|
|
for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if 50.0 <= time_s < 54.0
|
|
]
|
|
recovered = [
|
|
locked
|
|
for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if time_s >= 75.0
|
|
]
|
|
self.assertTrue(just_after_switch and not any(just_after_switch))
|
|
self.assertTrue(recovered and sum(recovered) / len(recovered) > 0.9)
|
|
self.assertGreaterEqual(trace.coarse_commits, 2)
|
|
self.assertEqual(trace.rejected_observations, 0)
|
|
|
|
def test_isolated_invalid_wls_does_not_reset_loop(self):
|
|
observations = noisy_observations(100.0, seed=61)
|
|
baseline = run_observations(observations, finish_time_s=100.0)
|
|
invalid = QualityMetrics(**{
|
|
**quality(Profile.PRECISION).__dict__,
|
|
"solution_valid": False,
|
|
"used_satellites": 2,
|
|
"velocity_valid": False,
|
|
})
|
|
observations = [
|
|
Observation(
|
|
item.time_s,
|
|
item.timing_error_s,
|
|
item.clock_drift_error_mps,
|
|
invalid if abs(item.time_s - 50.0) < 1.0e-9 else item.quality,
|
|
item.clock_drift_valid,
|
|
)
|
|
for item in observations
|
|
]
|
|
trace = run_observations(observations, finish_time_s=100.0)
|
|
around_reject = [
|
|
locked
|
|
for time_s, locked in zip(trace.time_s, trace.locked)
|
|
if 49.0 <= time_s <= 52.0 and abs(time_s - 50.0) > 1.0e-9
|
|
]
|
|
self.assertTrue(around_reject and all(around_reject))
|
|
self.assertEqual(trace.coarse_commits, baseline.coarse_commits)
|
|
|
|
def test_receiver_clock_drift_is_not_part_of_relative_wls_datum(self):
|
|
observations = noisy_observations(80.0, seed=67)
|
|
slow_clock = run_observations(
|
|
observations,
|
|
finish_time_s=80.0,
|
|
receiver_clock_drift_mps=0.0,
|
|
)
|
|
fast_clock = run_observations(
|
|
observations,
|
|
finish_time_s=80.0,
|
|
receiver_clock_drift_mps=900.0,
|
|
)
|
|
self.assertEqual(slow_clock.phase_error_s, fast_clock.phase_error_s)
|
|
self.assertEqual(
|
|
slow_clock.frequency_command_ppm,
|
|
fast_clock.frequency_command_ppm,
|
|
)
|
|
|
|
def test_fixed_point_profiles_fit_register_widths(self):
|
|
for profile in PROFILES.values():
|
|
kp, ki, slew = profile.fixed_coefficients(0.2)
|
|
self.assertLess(abs(kp), 1 << 47)
|
|
self.assertLess(abs(ki), 1 << 47)
|
|
self.assertLess(slew, 1 << 32)
|
|
self.assertLessEqual(profile.lock_release_ticks, 255)
|
|
self.assertLessEqual(profile.lock_acquire_updates, 255)
|
|
kp, ki, slew = ACQUIRE_PROFILE.fixed_coefficients(0.2)
|
|
self.assertLess(abs(kp), 1 << 47)
|
|
self.assertLess(abs(ki), 1 << 47)
|
|
self.assertLess(slew, 1 << 32)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|