From 156cfc8b77c7fa03796e13962f256739312724bb Mon Sep 17 00:00:00 2001 From: onur Date: Sun, 26 Jul 2026 03:15:52 +0300 Subject: [PATCH] fix(gps): adapt Doppler bandwidth from board validation --- hardware/external/gps/rtl/pps/README.md | 22 ++++ hardware/external/gps/rtl/trck_src/README.md | 27 +++++ soc/software/gps_measurement_python/README.md | 33 ++++-- .../analyze_doppler_eskf.py | 6 +- .../doppler_eskf_model.py | 2 +- .../output/DOPPLER_ESKF_ANALYSIS.md | 6 +- .../test_doppler_eskf_model.py | 64 +++++++++++ soc/software/gps_system/Makefile | 6 +- soc/software/gps_system/README.md | 100 ++++++++++++++---- soc/software/gps_system/gps_system.c | 26 ++++- .../header/fusion/navigation_fusion.h | 1 + .../header/gps/gps_interrupt_handlers.h | 2 + .../gps_system/header/gps/gps_receiver.h | 5 + .../header/gps/gps_tracking_measurement.h | 7 +- .../gps_system/src/adapters/gps_system_uart.c | 4 +- .../gps_system/src/fusion/navigation_fusion.c | 2 + .../src/gps/gps_interrupt_handlers.c | 34 +++++- soc/software/gps_system/src/gps/gps_ip.c | 68 +++++++++++- .../gps_system/src/gps/gps_receiver.c | 10 ++ .../src/gps/gps_tracking_measurement.c | 6 ++ .../tests/test_gps_pps_time_filter.c | 94 ++++++++++++++++ .../tests/test_gps_tracking_measurement.c | 8 ++ soc/software/pps_python/README.md | 24 +++++ soc/software/pps_python/test_pps_adaptive.py | 84 +++++++++++++++ 24 files changed, 592 insertions(+), 49 deletions(-) diff --git a/hardware/external/gps/rtl/pps/README.md b/hardware/external/gps/rtl/pps/README.md index d982f0c..172c7ec 100644 --- a/hardware/external/gps/rtl/pps/README.md +++ b/hardware/external/gps/rtl/pps/README.md @@ -57,6 +57,28 @@ This keeps quality policy and the small navigation pre-filter in software while the deterministic PI loop, fixed-point rate state and physical edge generation remain in hardware. +In a conventional absolute-pseudorange model, receiver clock bias and +receiver clock drift are the level and slope of the same physical clock: +`b_r = c*delta_t_r` and `bdot_r = c*d(delta_t_r)/dt`. The position-WLS bias +in this project is different: subtracting the common capture time and adding +`STARTOFFSET` makes it a reference-satellite-relative datum, approximately +`c*STARTOFFSET - reference_range`, rather than a pure receiver oscillator +offset. + +The Doppler WLS still solves a fourth, common receiver clock/frequency-drift +state. The PPS pre-filter does not use that state directly as the derivative +of the position-WLS datum. Instead, it predicts that datum with: + +```text +position_datum_drift = + -reference_geometric_range_rate + + c * reference_satellite_clock_drift +``` + +The geometric term uses the Doppler-WLS receiver velocity plus the selected +reference satellite position and velocity. The PPS RTL sees only the +resulting timestamped reference and loop configuration. + The APB register offsets live in `pps_pkg.sv`. Multiword input fields are shadow registers and must be followed by one `COMMIT` write. A new mailbox must not be written while `STATUS.COMMIT_BUSY` is set. diff --git a/hardware/external/gps/rtl/trck_src/README.md b/hardware/external/gps/rtl/trck_src/README.md index ee020ec..89e6459 100644 --- a/hardware/external/gps/rtl/trck_src/README.md +++ b/hardware/external/gps/rtl/trck_src/README.md @@ -89,6 +89,33 @@ observation = measured_range_rate H = [-u_x, -u_y, -u_z, 1] ``` +Buradaki dorduncu state, tum uydu Doppler observation'larina ortak giren +receiver clock/frequency drift'idir. Klasik mutlak-pseudorange modelinde +receiver zaman hatasi `delta_t_r(t)` icin: + +```text +clock_bias_m = c * delta_t_r(t) +clock_drift_mps = c * d(delta_t_r(t))/dt +clock_bias_m(t+dt) ~= clock_bias_m(t) + clock_drift_mps * dt +``` + +`clock_drift_mps`, receiver local oscillator frequency error'inin range-rate +birimindeki ortak etkisidir. `0.1 ppm`, yaklasik `29.98 m/s`'ye karsilik +gelir. Bu ortak state olmadan oscillator hatasi receiver hareketinin bir +parcasi sanilirdi. + +Bu projedeki position WLS bias'i ise mutlak receiver clock bias'i degildir. +Ortak capture zamani cikarildigi ve `STARTOFFSET` eklendigi icin +referans-uyduya bagli goreli bir datum cozer. Bu nedenle Doppler WLS +`clock_drift_mps` state'i, position WLS `clock_bias_m` degerinin turevi olarak +dogrudan baglanmaz. + +Iki WLS ayri observation ve normal equation kullanir; fakat Doppler WLS tam +bagimsiz degildir. Uydu LOS vektorlerini kurmak icin position WLS'den gelen +guncel receiver ECEF konumunu, ephemeristen gelen uydu ECEF konum/hiz ve clock +drift'ini kullanir. Satellite clock drift bilinen duzeltmedir; WLS'nin +dorduncu bilinmeyeni receiver clock drift'idir. + WLS en az dort gecerli uydu ister; ECEF velocity, 3x3 covariance, receiver clock drift ve residual RMS uretir. Fusion adapter velocity/covariance'i ENU'ya dondurur ve ayri velocity NIS kapisi ile 15-state ESKF'ye verir. diff --git a/soc/software/gps_measurement_python/README.md b/soc/software/gps_measurement_python/README.md index 09f85e0..f210f59 100644 --- a/soc/software/gps_measurement_python/README.md +++ b/soc/software/gps_measurement_python/README.md @@ -90,10 +90,11 @@ modelinin açıklayamadığı kanal-bağımlı excess jitter'ı şişirir. Ayrı 1D model ham code, kör scalar IIR ve Doppler-predicted code smoothing'i range-rate adımları ve residual carrier CFO altında karşılaştırır. Carrier ölçümleri RTL cadence'i olan 50 Hz'de üretilir ve 200 ms WLS epoch'una kadar -entegre edilir. Hem raw carrier-rate hem de mevcut firmware'deki -`GPS_TRACK_DOPPLER_SMOOTH_ALPHA=0.05` çıkışı denenir. Bu ayrım önemlidir: +entegre edilir. Ham, sabit `alpha=0.05` ve IMU hareket durumuna göre bant +genişliğini açan adaptif Doppler yolları birlikte denenir. Bu ayrım önemlidir: PPS datum-drift predictor'u için yararlı olan yavaş Doppler filtresi, ESKF -velocity veya hızlı code-aiding prediction'ında dinamik lag üretebilir. +velocity veya hızlı code-aiding prediction'ında dinamik lag üretebilir; tam +ham snapshot ise kartta ölçülen gürültü seviyesinde fazla varyans üretebilir. ## Doppler pre-IIR ve ESKF modeli @@ -121,7 +122,7 @@ Senaryo matrisi sabit uçuş, airliner/aggressive manevra, düşük C/N0, tek-ka Doppler burst, common receiver clock-drift step, 15 saniye GPS kesintisi ve birleşik arıza koşullarını içerir. -20-seed sentetik sonuçların özeti: +Nominal 20-seed sentetik sonuçların özeti: - ESKF ham WLS gürültüsünü nominalde yaklaşık `0.416 m/s` 3D RMS'ten `0.190 m/s` seviyesine indirir; temporal filtreleme zaten ESKF'de gerçekleşir. @@ -136,11 +137,25 @@ birleşik arıza koşullarını içerir. - Common receiver clock-frequency hatası WLS'nin dördüncü clock-drift state'i tarafından hızdan ayrılır. -Bu sonuçlarla firmware'de ESKF için ham Doppler -> kanal validity/FDE -> -velocity WLS + covariance -> ESKF yolu uygulanmıştır. Ayrı yavaş-filtered -Doppler velocity yalnız PPS position-WLS clock-bias datumunun drift predictor'u -ve PPS kalite metriği için tutulur. Ayrıntılı tablo ve sınırlar -`output/DOPPLER_ESKF_ANALYSIS.md` içindedir. +Genesys2 sabit-kart ölçümünde tam ham yolun velocity-WLS RMS'i `9.17 m/s`, +p95'i `16.71 m/s` ve FDE oranı `86/120` oldu. Aynı epoch'larda sabit +`alpha=0.05` yol `0.92 m/s` RMS, `1.63 m/s` p95 ve `0/120` FDE verdi. Bu +nedenle nominal modelde iyi görünen doğrudan ham yol production'a alınmadı. +Kart gürültüsünü eşlemek için tracking noise `22x` ile kalibre edilen regresyon +modelindeki regresyon seed'leri için steady-7/airliner-3/aggressive-7 fused +RMS değerleri: + +- sabit IIR: `0.451 / 0.484 / 1.036 m/s`, +- adaptif IIR: `0.451 / 0.336 / 0.538 m/s`, +- tam ham: `1.260 / 1.100 / 1.260 m/s`. + +Production firmware ESKF için `alpha=0.05/0.20/0.40` hareket-adaptif Doppler +-> bandwidth-aware sigma -> kanal validity/FDE -> velocity WLS + covariance +-> ESKF yolunu kullanır. Ham alan tanısal kayıt ve offline analiz için korunur. +Ayrı sabit `alpha=0.05` Doppler velocity yalnız PPS position-WLS clock-bias +datumunun drift predictor'u ve PPS kalite metriği için kullanılır. Ayrıntılı +nominal tablo ve model sınırları `output/DOPPLER_ESKF_ANALYSIS.md` içindedir; +kart-kalibreli karar `test_doppler_eskf_model.py` ile regresyona alınmıştır. ## Kurulum ve çalıştırma diff --git a/soc/software/gps_measurement_python/analyze_doppler_eskf.py b/soc/software/gps_measurement_python/analyze_doppler_eskf.py index 126189f..94eb83d 100644 --- a/soc/software/gps_measurement_python/analyze_doppler_eskf.py +++ b/soc/software/gps_measurement_python/analyze_doppler_eskf.py @@ -348,10 +348,10 @@ def write_report(path: Path, summaries: list[Summary], seed_count: int) -> None: "", "## Mimari kararı", "", - "- ESKF açıkken uygulanan ana yol: 20 ms ham Doppler snapshot -> kanal validity/outlier kontrolü -> 5 Hz robust velocity WLS + gerçek covariance -> adapter tabanları -> ESKF. Sabit `alpha=0.05` temporal IIR yalnız PPS drift-predictor yolunda kalır.", + "- Nominal sentetik model ham Doppler'in sabit pre-IIR gecikmesini önlediğini gösterir; fakat Genesys2 kart-kalibreli noise regresyonunda tam ham yol fazla gürültülüdür. Production ana yolu: hareket-adaptif Doppler -> bandwidth-aware sigma -> kanal validity/outlier kontrolü -> 5 Hz robust velocity WLS + gerçek covariance -> adapter tabanları -> ESKF.", "- ESKF yalnız 3D aggregate velocity gördüğü için tek uydu hatasını sonradan teşhis edemez. Per-satellite robust gate/FDE WLS öncesinde kalmalıdır.", - "- Standalone GPS speed, PPS aiding veya ESKF kapalı build için ayrı, yavaş filtrelenmiş Doppler çıktısı tutulabilir. Aynı filtered değeri hem code aiding hem ESKF velocity için kullanmak uygun değildir.", - "- Adaptif IIR mümkündür; modelde C/N0 sakin-state alpha'sını, düşük geçiren IMU navigation acceleration ise dynamic alpha'yı seçer. Ancak ESKF'li ana yol için ham ölçüm daha sade ve gecikmesizdir.", + "- Ayrı sabit `alpha=0.05` yol PPS drift predictor ve standalone sakin-hız kalite metriği için tutulur; ham snapshot tanısal/offline analiz için korunur.", + "- Production adaptif IIR sakin/hareketli/yüksek-ivme için `alpha=0.05/0.20/0.40` seçer. Alpha büyüdükçe EWMA noise gain'i WLS sigma'sına yansıtılır; böylece FDE ve ESKF covariance bant genişliğini bilir.", "", "## Model sınırları", "", diff --git a/soc/software/gps_measurement_python/doppler_eskf_model.py b/soc/software/gps_measurement_python/doppler_eskf_model.py index 54849fd..2d43620 100644 --- a/soc/software/gps_measurement_python/doppler_eskf_model.py +++ b/soc/software/gps_measurement_python/doppler_eskf_model.py @@ -4,7 +4,7 @@ The model compares four paths using one shared 50 Hz tracking stream and one shared 100 Hz IMU stream: * the legacy fixed-alpha Doppler IIR before 5 Hz velocity WLS; -* raw Doppler into WLS, leaving temporal filtering to the ESKF; +* raw Doppler into WLS, as a nominal-model comparison rather than production; * a global IMU-driven adaptive-alpha IIR; * raw Doppler with the per-satellite robust WLS gate used before the ESKF. diff --git a/soc/software/gps_measurement_python/output/DOPPLER_ESKF_ANALYSIS.md b/soc/software/gps_measurement_python/output/DOPPLER_ESKF_ANALYSIS.md index 55df3c4..ecd2b00 100644 --- a/soc/software/gps_measurement_python/output/DOPPLER_ESKF_ANALYSIS.md +++ b/soc/software/gps_measurement_python/output/DOPPLER_ESKF_ANALYSIS.md @@ -50,10 +50,10 @@ Monte Carlo seed sayısı: `20`; ilk 5 saniye RMS metriklerinden çıkarıldı. ## Mimari kararı -- ESKF açıkken uygulanan ana yol: 20 ms ham Doppler snapshot -> kanal validity/outlier kontrolü -> 5 Hz robust velocity WLS + gerçek covariance -> adapter tabanları -> ESKF. Sabit `alpha=0.05` temporal IIR yalnız PPS drift-predictor yolunda kalır. +- Nominal sentetik model ham Doppler'in sabit pre-IIR gecikmesini önlediğini gösterir; fakat Genesys2 kart-kalibreli noise regresyonunda tam ham yol fazla gürültülüdür. Production ana yolu: hareket-adaptif Doppler -> bandwidth-aware sigma -> kanal validity/outlier kontrolü -> 5 Hz robust velocity WLS + gerçek covariance -> adapter tabanları -> ESKF. - ESKF yalnız 3D aggregate velocity gördüğü için tek uydu hatasını sonradan teşhis edemez. Per-satellite robust gate/FDE WLS öncesinde kalmalıdır. -- Standalone GPS speed, PPS aiding veya ESKF kapalı build için ayrı, yavaş filtrelenmiş Doppler çıktısı tutulabilir. Aynı filtered değeri hem code aiding hem ESKF velocity için kullanmak uygun değildir. -- Adaptif IIR mümkündür; modelde C/N0 sakin-state alpha'sını, düşük geçiren IMU navigation acceleration ise dynamic alpha'yı seçer. Ancak ESKF'li ana yol için ham ölçüm daha sade ve gecikmesizdir. +- Ayrı sabit `alpha=0.05` yol PPS drift predictor ve standalone sakin-hız kalite metriği için tutulur; ham snapshot tanısal/offline analiz için korunur. +- Production adaptif IIR sakin/hareketli/yüksek-ivme için `alpha=0.05/0.20/0.40` seçer. Alpha büyüdükçe EWMA noise gain'i WLS sigma'sına yansıtılır; böylece FDE ve ESKF covariance bant genişliğini bilir. ## Model sınırları diff --git a/soc/software/gps_measurement_python/test_doppler_eskf_model.py b/soc/software/gps_measurement_python/test_doppler_eskf_model.py index bf678ee..5944fca 100644 --- a/soc/software/gps_measurement_python/test_doppler_eskf_model.py +++ b/soc/software/gps_measurement_python/test_doppler_eskf_model.py @@ -2,6 +2,7 @@ from __future__ import annotations +from dataclasses import replace import unittest import numpy as np @@ -42,6 +43,19 @@ def _wls_rms(trace) -> float: return float(np.sqrt(np.mean(np.sum(np.square(error), axis=1)))) +def _board_raw_noise_scenario(name: str): + """Match the 9.17 m/s raw-WLS RMS measured on Genesys2.""" + scenario = _scenario(name) + nominal_noise = scenario.noise_multiplier + return replace( + scenario, + name=f"{scenario.name}_genesys2_noise", + noise_multiplier=lambda time_s, satellite: ( + 22.0 * nominal_noise(time_s, satellite) + ), + ) + + class DopplerWlsTest(unittest.TestCase): def test_four_state_wls_separates_velocity_and_clock_drift(self) -> None: velocity = np.asarray([145.0, -32.0, 7.5]) @@ -85,6 +99,56 @@ class AdaptiveAlphaTest(unittest.TestCase): class DopplerEskfIntegrationTest(unittest.TestCase): + def test_board_raw_noise_requires_adaptive_prefilter(self) -> None: + steady = run_doppler_eskf_scenario( + _board_raw_noise_scenario("steady_cruise"), + seed=7, + paths=( + DopplerPath.FIXED_IIR, + DopplerPath.ADAPTIVE_IIR, + DopplerPath.RAW, + ), + ) + airliner = run_doppler_eskf_scenario( + _board_raw_noise_scenario("airliner_maneuver"), + seed=3, + paths=( + DopplerPath.FIXED_IIR, + DopplerPath.ADAPTIVE_IIR, + DopplerPath.RAW, + ), + ) + aggressive = run_doppler_eskf_scenario( + _board_raw_noise_scenario("aggressive_maneuver"), + seed=7, + paths=( + DopplerPath.FIXED_IIR, + DopplerPath.ADAPTIVE_IIR, + DopplerPath.RAW, + ), + ) + steady_fixed = _fused_rms(steady[DopplerPath.FIXED_IIR]) + steady_adaptive = _fused_rms(steady[DopplerPath.ADAPTIVE_IIR]) + steady_raw = _fused_rms(steady[DopplerPath.RAW]) + airliner_fixed = _fused_rms(airliner[DopplerPath.FIXED_IIR]) + airliner_adaptive = _fused_rms(airliner[DopplerPath.ADAPTIVE_IIR]) + airliner_raw = _fused_rms(airliner[DopplerPath.RAW]) + aggressive_fixed = _fused_rms(aggressive[DopplerPath.FIXED_IIR]) + aggressive_adaptive = _fused_rms( + aggressive[DopplerPath.ADAPTIVE_IIR] + ) + aggressive_raw = _fused_rms(aggressive[DopplerPath.RAW]) + + # Direct 20 ms snapshots are too noisy at the level actually observed + # on the board. Adaptive bandwidth retains the quiet-state result and + # beats both direct raw and the 0.39 s fixed-IIR lag in manoeuvres. + self.assertLess(steady_adaptive, 1.05 * steady_fixed) + self.assertLess(steady_adaptive, 0.50 * steady_raw) + self.assertLess(airliner_adaptive, airliner_fixed) + self.assertLess(airliner_adaptive, 0.50 * airliner_raw) + self.assertLess(aggressive_adaptive, 0.65 * aggressive_fixed) + self.assertLess(aggressive_adaptive, 0.50 * aggressive_raw) + def test_eskf_filters_raw_doppler_without_pre_iir(self) -> None: traces = run_doppler_eskf_scenario( _scenario("steady_cruise"), diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index a187768..f983741 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -82,6 +82,8 @@ BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c GPS_NAV_SOLVE_TIMER_TICKS ?= 10 GPS_PPS_STATUS_LOG_INTERVAL ?= 10 +GPS_PPS_DOPPLER_PREDICTOR_ENABLE ?= 1 +GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES ?= 0 include ../common/app.mk @@ -94,7 +96,9 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \ -DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \ -DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \ -DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) \ - -DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL) + -DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL) \ + -DGPS_PPS_DOPPLER_PREDICTOR_ENABLE=$(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) \ + -DGPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=$(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) LDLIBS += -lm ifeq ($(GPS_DEBUG),1) diff --git a/soc/software/gps_system/README.md b/soc/software/gps_system/README.md index 3cb7614..80f88ce 100644 --- a/soc/software/gps_system/README.md +++ b/soc/software/gps_system/README.md @@ -176,6 +176,24 @@ Iki komut ayni MCS/BIN yoluna yazdigi icin iki imaj birlikte saklanacaksa build'ler arasinda profil adiyla kopyalanmalidir. Bu secenekler software derleme parametresidir; mevcut bitstream ile `fpga-mcs` yeterlidir. +PPS Doppler datum-drift predictor production'da varsayilan olarak aciktir. +Kontrollu A/B icin build-time secenekleri: + +```sh +# Production: predictor sürekli acik, otomatik A/B yok. +make -C soc/software/gps_system \ + GPS_PPS_DOPPLER_PREDICTOR_ENABLE=1 \ + GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=0 + +# Tani: her 200 navigation update'inda (~40 s) kapali/acik degistir. +make -C soc/software/gps_system \ + GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=200 +``` + +`GPS_PPS_DOPPLER_PREDICTOR_ENABLE=0` yalniz kontrollu fallback testi icindir. +`GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES` sifirdan buyukse A/B secimi enable +degerini bilerek ezer; production image'da mutlaka `0` kalmalidir. + Otomatik WHO_AM_I algilama varsayilandir: ```sh @@ -812,15 +830,20 @@ range_rate_mps = -doppler_hz * c / 1575420000 Ham 20 ms carrier snapshot'inda kartta gorulen birkac hertz jitter, L1'de metre/saniye seviyesinde range-rate gurultusune ve zayif geometride daha buyuk receiver velocity hatasina donusur. Software ISR her PRN icin ham degeri -degistirmeden korur ve yalniz PPS predictor yolu icin ayri bir state'te: +degistirmeden korur ve iki ayri state gunceller: ```text -doppler_filtered = 0.05 * doppler_raw + 0.95 * doppler_previous +PPS: doppler_slow = 0.05 * raw + 0.95 * previous +ESKF: doppler_fast = alpha * raw + (1-alpha) * previous + alpha = 0.05 stationary, 0.20 moving, 0.40 high acceleration ``` -hesaplar. Ham ve filtreli range-rate ayri alanlarda tutulur. 50 Hz measurement -cadence'inde bu yaklasik `0.39 s` zaman sabitidir. Reacquisition/PRN -degisiminde eski kanal state'i kullanilmaz. +PPS yolu 50 Hz cadence'de yaklasik `0.39 s` zaman sabitine sahiptir. ESKF yolu +fusion motion state ve gravity-compensated linear acceleration ile bant +genisligini acar. Alpha buyudugunde EWMA noise kazanci kadar WLS range-rate +sigma da buyutulur; bu, FDE ve ESKF covariance'in filtre bant genisliginden +habersiz kalmasini engeller. Reacquisition/PRN degisiminde eski kanal state'i +kullanilmaz. IIR donanimda da uygulanabilirdi; bunun icin kanal basina state, coefficient-multiply, rounding/saturation ve reacquisition reset kurali @@ -847,12 +870,13 @@ weight = 1 / sigma_rate^2 Her 200 ms navigation epoch'unda iki velocity tuketicisi vardir: ```text -raw range-rate -> robust velocity WLS -> fusion adapter -> ESKF -filtered range-rate -> robust velocity WLS -> PPS bias-drift predictor +adaptive range-rate -> robust velocity WLS -> fusion adapter -> ESKF +slow range-rate -> robust velocity WLS -> PPS bias-drift predictor ``` -Ham ESKF yolu temporal filtreleme isini ESKF'ye birakir ve `alpha=0.05` -IIR'in yaklasik 390 ms dinamik gecikmesini velocity measurement'a eklemez. +Tam ham 20 ms snapshot yolu kart A/B'sinde fazla gurultulu bulundugu icin +ESKF'ye yayinlanmaz. Adaptif yol sakin durumda PPS kadar dusuk gurultulu, +harekette ise sabit `alpha=0.05` yolundan daha dusuk gecikmelidir. PPS'in ana olcumu position WLS `clock_bias_m` sonucudur. Filtreli velocity yalniz bu referans-datum bias'inin 200 ms epoch'lar arasindaki turevini ongormek ve PPS kalite profilini siniflandirmak icin kullanilir. Doppler @@ -905,9 +929,8 @@ velocity/covariance, `speed_mps`, `velocity_residual_rms_mps`, Fusion adapter covariance'i `R_ecef_to_enu * P * R^T` ile ENU'ya dondurur. 2026-07-25 sabit kart verisinde zayif geometriyle gorulen `p95=6.020 m/s` speed hatasi nedeniyle default alt sinirlar yatay eksenlerde -`2.0 m/s`, dikey eksende `3.0 m/s`'dir. ESKF artik ham Doppler WLS aldigi icin -bu konservatif tabanlar temporal korelasyondan cok zayif geometri ve kartta -henuz yeniden kalibre edilmemis raw-Doppler hata kuyruklarini sinirlar; formal +`2.0 m/s`, dikey eksende `3.0 m/s`'dir. Bu konservatif tabanlar zayif geometri +ve kartta kalan Doppler hata kuyruklarini sinirlar; formal covariance daha buyukse aynen korunur. Velocity innovation 3-DOF `%99.9` `NIS=16.266` kapisindan gecer. Kabul/red sayilari ve son velocity NIS'i `[FUSION]` UART status satirinda `velocity=accepted/rejected` ve `vel_nis` @@ -964,9 +987,9 @@ GPS_LOG(level, category, ...) | `GPS_DEBUG_LEVEL` | Eklenen cikti | | ---: | --- | -| `1` | Temel telemetri: ham GPS/WLS `[LOCATION] source=gps` satirinda DOP ile birlikte `velocity_valid`, `speed` ve `velocity_rms`; gecerliyse ESKF `[LOCATION] source=imu_corrected`; sifir tabanli donanim kanal numaralarini tasiyan `[TRK] tracked satellites=... prn_indices=[...]` (`0` = GPS PRN `G01`). Baslangicta fusion/IMU konfigurasyonu da bir kez yazilir. | +| `1` | Temel telemetri: ham GPS/WLS `[LOCATION] source=gps` satirinda DOP ile birlikte `velocity_valid`, `speed` ve `velocity_rms`; gecerliyse ESKF `[LOCATION] source=imu_corrected`; sifir tabanli donanim kanal numaralarini tasiyan `[TRK] tracked satellites=... prn_indices=[...]` (`0` = GPS PRN `G01`). Baslangicta fusion/IMU konfigurasyonu ve periyodik `[FUSION]` NIS/kabul-red durumu da gorulur. | | `2` | Gelecekteki ek status ayrintilari icin ayrilmistir. Su an level 2'ye ozel mesaj yoktur; pratikte level 1 ile ayni ciktiyi verir. | -| `3` | Level 1'e ek olarak acquisition/tracking/navigation durum gecisleri, warning/error mesajlari, integrity bilgileri ve periyodik ayrintili `[FUSION] state=...` durumu. | +| `3` | Level 1'e ek olarak acquisition/tracking/navigation durum gecisleri, warning/error mesajlari ve integrity bilgileri. | | `4` | Level 3'e ek olarak performans, scheduler, nav-bit/parser, uydu/olcum ve dusuk seviye TRACE tanilari. UART yuku en yuksek seviyedir. | `Makefile` varsayilanlari `GPS_DEBUG=1` ve `GPS_DEBUG_LEVEL=4`'tur. Bu nedenle @@ -1234,10 +1257,51 @@ azaltiyor ve C/N0 weighting ornek epoch'ta sonucu iyilestiriyor. Buna ragmen position WLS'deki pseudorange/code-phase hata ve zayif geometriyi tek basina duzeltmiyor. Doppler velocity artik ESKF state update'ine baglidir; formal covariance, kart verisine dayali floor ve ayri NIS gate ile asiri guven -sinirlanir. Bu yeni software'in gercek kartta `vel_nis` ve -`velocity=accepted/rejected` sayaclariyla saha A/B sonucu henuz olculmedi; -yukaridaki board istatistikleri ESKF entegrasyonundan onceki ham velocity -dogrulamasidir. +sinirlanir. Yukaridaki board istatistikleri ESKF entegrasyonundan onceki +velocity dogrulamasidir; entegrasyon sonrasi `vel_nis` ve +`velocity=accepted/rejected` saha sonucu bir sonraki bolumdedir. + +### 2026-07-26 Genesys2 PPS ve Doppler/ESKF A/B + +Kart sabitken once tam ham 20 ms snapshot -> robust WLS -> ESKF yolu olculdu. +Son 120 PPS penceresinde ham WLS speed RMS `9.17 m/s`, p95 `16.71 m/s` ve +raw-FDE `86/120` oldu. Yavas `alpha=0.05` PPS yolu ayni pencerede `0.92 m/s` +RMS, `1.63 m/s` p95 ve `0/120` FDE verdi. ESKF+ZUPT fused hizi dusuk tutsa da +bu sonuc tam ham yolun hareketli platform icin guvenli oldugunu dogrulamadi. + +Bu nedenle ESKF icin yukarida tarif edilen ayri adaptif yol uygulandi. Ayni +kartta 240 epoch'luk yerlesmis pencerede `alpha=0.05`, speed RMS `0.48 m/s`, +p95 `0.83 m/s`, FDE `0/240` olculdu. Son 180 saniyede ESKF velocity kabul/red +farki `892/0`, velocity NIS median/p95/max `0.028/0.111/0.180`, fused speed +RMS `0.0052 m/s` oldu. IMU overflow sayaci bu pencerede `15` olarak sabit +kaldi; olcum telemetrisi yeni kayip uretmedi. + +PPS predictor, yeniden boot etmeden 40 saniyelik kapali/acik segmentlerle A/B +yapildi (`GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES=200`, yalniz tanisal build). +Her segmentin ilk bes saniyesi atildiginda predictor acik segmentlerde software +clock-bias innovation RMS `12.52 m`, kapali segmentlerde `46.48 m` oldu. +Hardware phase p95 acikken `51.02 ns`, kapaliyken `58.21 ns` idi. Tek acik +segmentte gorulen bir `-216.48 ns` one-epoch phase transient'i lock kaybi +olusturmadi; bu nedenle physical-pin jitter sonucu olarak yorumlanmaz. +Production default predictor acik, A/B toggling kapali (`0`) kalir. + +Son olarak `adaptive-pps-final-20260726` production image'i +`PREDICTOR_ENABLE=1`, `PREDICTOR_AB_UPDATES=0` ile RAM'den karta yuklendi. +Acquisition/coarse baslangici dislandiktan sonraki 120 lock status orneginde: + +- predictor tum orneklerde `1`, ESKF alpha sabit kartta tum orneklerde `0.05`, +- adaptive velocity-WLS speed RMS/p95 `0.52/0.92 m/s`, FDE `0/120`, +- ESKF velocity kabul/red farki `448/0`, velocity NIS p95/max + `0.139/0.341`, fused speed RMS `0.0061 m/s`, +- PPS software clock innovation RMS `12.36 m`, +- internal hardware phase RMS ve p95-absolute `28.50/58.36 ns`, lock kaybi + `0/120`, +- IMU overflow sayaci `13..13`, tracking measurement missed sayaci `0..0` + kaldi. + +Bu `hw_phase`, APB status'taki son commit residual'idir; fiziksel PPS pin +jitter'i degildir. Dolayisiyla `15-20 ns` pin hedefinin saglandigi bu logdan +iddia edilmez; scope veya time-interval counter ile ayri olculmelidir. ### 2026-07-08 ACQ Debug Notu diff --git a/soc/software/gps_system/gps_system.c b/soc/software/gps_system/gps_system.c index c9b1ae8..3ef74bc 100644 --- a/soc/software/gps_system/gps_system.c +++ b/soc/software/gps_system/gps_system.c @@ -29,6 +29,22 @@ #define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u #define IMU_SAMPLES_AFTER_GPS_SERVICE 3u +static void update_eskf_doppler_bandwidth( + const navigation_fusion_output_t* output) +{ + if((output == NULL) || !output->local_valid) + { + return; + } + double alpha = 0.05; + if(output->motion_state == FUSION_MOTION_MOVING) + { + alpha = (output->linear_acceleration_norm_mps2 >= 2.5) ? + 0.40 : 0.20; + } + gps_receiver_set_eskf_doppler_alpha(alpha); +} + static void initialize_sensor_fusion(gps_system_state_t* system) { system->fusion_enabled = (GPS_ESKF_ENABLE != 0); @@ -69,9 +85,13 @@ static unsigned int feed_pending_imu_to_eskf( imu_sensor_take_measurement(&system->imu_sample)) { bool accepted = navigation_fusion_push_imu(&system->imu_sample); - if(accepted && - navigation_fusion_get_output(&system->fused_solution) && - system->fused_solution.global_valid && + bool output_available = accepted && + navigation_fusion_get_output(&system->fused_solution); + if(output_available) + { + update_eskf_doppler_bandwidth(&system->fused_solution); + } + if(output_available && system->fused_solution.global_valid && (system->fused_solution.timestamp_ticks != system->last_fused_location_tick)) { diff --git a/soc/software/gps_system/header/fusion/navigation_fusion.h b/soc/software/gps_system/header/fusion/navigation_fusion.h index 0c45759..be44b70 100644 --- a/soc/software/gps_system/header/fusion/navigation_fusion.h +++ b/soc/software/gps_system/header/fusion/navigation_fusion.h @@ -19,6 +19,7 @@ typedef struct { double height_m; double position_std_m[3]; double speed_mps; + double linear_acceleration_norm_mps2; double last_position_nis; double last_velocity_nis; fusion_motion_state_t motion_state; diff --git a/soc/software/gps_system/header/gps/gps_interrupt_handlers.h b/soc/software/gps_system/header/gps/gps_interrupt_handlers.h index 4599b4f..00e55ca 100644 --- a/soc/software/gps_system/header/gps/gps_interrupt_handlers.h +++ b/soc/software/gps_system/header/gps/gps_interrupt_handlers.h @@ -55,6 +55,8 @@ void gps_background_task(void); bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot); void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics); + void gps_tracking_set_eskf_doppler_alpha(double alpha); + double gps_tracking_get_eskf_doppler_alpha(void); void trck_intr_handler(volatile gps_trck_t* const trck_ip, volatile gps_frame_t *gps_frame_parsed, volatile int unsigned gps_subframe[10], unsigned char trck_ip_no); diff --git a/soc/software/gps_system/header/gps/gps_receiver.h b/soc/software/gps_system/header/gps/gps_receiver.h index 472c308..47e3a86 100644 --- a/soc/software/gps_system/header/gps/gps_receiver.h +++ b/soc/software/gps_system/header/gps/gps_receiver.h @@ -38,4 +38,9 @@ bool gps_receiver_solve_ready_epoch( uint64_t gps_receiver_status_ticks(void); uint64_t gps_receiver_status_ticks_per_second(void); +/* Select the low-latency Doppler EWMA used only by velocity WLS/ESKF. + * PPS keeps its independent fixed low-noise path. */ +void gps_receiver_set_eskf_doppler_alpha(double alpha); +double gps_receiver_get_eskf_doppler_alpha(void); + #endif diff --git a/soc/software/gps_system/header/gps/gps_tracking_measurement.h b/soc/software/gps_system/header/gps/gps_tracking_measurement.h index 2be86c8..5e3e7ec 100644 --- a/soc/software/gps_system/header/gps/gps_tracking_measurement.h +++ b/soc/software/gps_system/header/gps/gps_tracking_measurement.h @@ -6,6 +6,8 @@ #include "gps/gps_ip_defines.h" +#define GPS_TRACK_PPS_DOPPLER_ALPHA 0.05 + typedef struct { uint32_t prn; uint32_t sequence; @@ -17,9 +19,12 @@ typedef struct { int32_t cn0_sum_q; uint64_t cn0_wbp; unsigned int cn0_sample_count; - /* Raw coherent 20 ms carrier snapshot used by velocity WLS/ESKF. */ + /* Raw coherent 20 ms carrier snapshot retained for diagnostics. */ double doppler_hz; double pseudorange_rate_mps; + /* Motion-adaptive low-latency path published to velocity WLS/ESKF. */ + double eskf_doppler_hz; + double eskf_pseudorange_rate_mps; /* Slow software IIR path retained only for PPS bias-drift prediction. */ double filtered_doppler_hz; double filtered_pseudorange_rate_mps; diff --git a/soc/software/gps_system/src/adapters/gps_system_uart.c b/soc/software/gps_system/src/adapters/gps_system_uart.c index cc09bfb..9f1d485 100644 --- a/soc/software/gps_system/src/adapters/gps_system_uart.c +++ b/soc/software/gps_system/src/adapters/gps_system_uart.c @@ -115,7 +115,7 @@ void gps_system_uart_publish_periodic_fusion_status( if(output->global_valid) { - GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION, + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION, "state=%s lat=%.10lf lon=%.10lf height=%.3lf " "speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf " "gps=%lu/%lu velocity=%lu/%lu recoveries=%lu " @@ -142,7 +142,7 @@ void gps_system_uart_publish_periodic_fusion_status( return; } - GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION, + GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION, "state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu " "imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n", navigation_fusion_state_name(output->engine_state), diff --git a/soc/software/gps_system/src/fusion/navigation_fusion.c b/soc/software/gps_system/src/fusion/navigation_fusion.c index a1b53fd..5c28764 100644 --- a/soc/software/gps_system/src/fusion/navigation_fusion.c +++ b/soc/software/gps_system/src/fusion/navigation_fusion.c @@ -141,6 +141,8 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output) sqrt(fmax(0.0, runtime->filter.covariance[axis][axis])); } output->speed_mps = eskf_speed_mps(&runtime->filter); + output->linear_acceleration_norm_mps2 = + runtime->motion.last_linear_accel_norm_mps2; output->last_position_nis = runtime->filter.last_position_nis; output->last_velocity_nis = runtime->filter.last_velocity_nis; output->motion_state = runtime->motion.state; diff --git a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c index 660196c..07644a0 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -9,6 +9,7 @@ #include #include #include +#include #include typedef enum { @@ -91,9 +92,13 @@ static gps_external_irq_handler_t gps_external_irq_handler = NULL; (((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u) #define GPS_NAV_HOLDOFF_LOCAL_TICKS \ (((uint64_t)BSP_CLINT_TIMEBASE_HZ * GPS_NAV_EPOCH_HOLDOFF_MS) / 1000u) -#define GPS_TRACK_DOPPLER_SMOOTH_ALPHA 0.05 +#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN 0.05 +#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX 0.40 #define GPS_NAV_TOW_SNAP_TOLERANCE_SEC 0.005 +static volatile double gps_tracking_eskf_doppler_alpha = + GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN; + _Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS < (GPS_NAV_SOLVE_PERIOD_MS / 2u), "NAV capture window must be below half the solve period"); @@ -112,6 +117,22 @@ static unsigned int gps_mask_count(uint32_t mask) return count; } +void gps_tracking_set_eskf_doppler_alpha(double alpha) +{ + if(!isfinite(alpha)) + { + return; + } + gps_tracking_eskf_doppler_alpha = fmax( + GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN, + fmin(GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX, alpha)); +} + +double gps_tracking_get_eskf_doppler_alpha(void) +{ + return gps_tracking_eskf_doppler_alpha; +} + static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phase) { return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS); @@ -157,12 +178,19 @@ static void gps_update_tracking_measurement_isr( * 0.4 s time constant while retaining real receiver dynamics. */ measurement.filtered_doppler_hz = - GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz + - (1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) * + GPS_TRACK_PPS_DOPPLER_ALPHA * measurement.doppler_hz + + (1.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) * previous->filtered_doppler_hz; measurement.filtered_pseudorange_rate_mps = gps_tracking_doppler_to_range_rate_mps( measurement.filtered_doppler_hz); + double eskf_alpha = gps_tracking_eskf_doppler_alpha; + measurement.eskf_doppler_hz = + eskf_alpha * measurement.doppler_hz + + (1.0 - eskf_alpha) * previous->eskf_doppler_hz; + measurement.eskf_pseudorange_rate_mps = + gps_tracking_doppler_to_range_rate_mps( + measurement.eskf_doppler_hz); } measurement.local_timestamp_ticks = local_timestamp_ticks; gps_latest_tracking_measurement[tracking_index] = measurement; diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index 8cd0285..745bfb9 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -69,11 +69,25 @@ static bool gps_pps_reference_svid_valid; static unsigned int gps_pps_reference_svid; static bool gps_pps_filter_reference_svid_valid; static unsigned int gps_pps_filter_reference_svid; +#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0 +static uint64_t gps_pps_predictor_ab_update_counter; +#endif #if !defined(GPS_PPS_STATUS_LOG_INTERVAL) || (GPS_PPS_STATUS_LOG_INTERVAL < 1) #error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one" #endif +#if !defined(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) || \ + ((GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) && \ + (GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 1)) +#error "GPS_PPS_DOPPLER_PREDICTOR_ENABLE must be zero or one" +#endif + +#if !defined(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) || \ + (GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES < 0) +#error "GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES must be zero or positive" +#endif + volatile uint64_t gps_systick_ticks = 0; static void gps_init_acq_tables(void) @@ -854,6 +868,9 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, bool pps_clock_bias_drift_valid = false; double pps_clock_bias_drift_mps = 0.0; bool pps_velocity_valid = false; + double eskf_doppler_alpha = + gps_tracking_get_eskf_doppler_alpha(); + double pps_velocity_speed_mps = NAN; double pps_velocity_residual_rms_mps = 0.0; uint32_t pps_velocity_excluded_row_mask = 0u; gps_nav_solution_t solution; @@ -964,6 +981,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, } if(ok && solution.solution_valid) { + double reference_noise_ratio = + eskf_doppler_alpha * + (2.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) / + (GPS_TRACK_PPS_DOPPLER_ALPHA * + (2.0 - eskf_doppler_alpha)); + double eskf_cn0_penalty_db = + 10.0 * log10(fmax(1.0, reference_noise_ratio)); double satellite_positions_array[GPS_TRCK_IP_COUNT][3]; double satellite_velocities_array[GPS_TRCK_IP_COUNT][3]; gps_tracking_measurement_t @@ -978,6 +1002,12 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, velocities->matrix[row][component]; } velocity_measurements[row] = measurements[row]; + velocity_measurements[row].doppler_hz = + measurements[row].eskf_doppler_hz; + velocity_measurements[row].pseudorange_rate_mps = + measurements[row].eskf_pseudorange_rate_mps; + velocity_measurements[row].cn0_dbhz -= + eskf_cn0_penalty_db; if((solution.used_row_mask & (1u << row)) == 0u) { velocity_measurements[row].valid = false; @@ -1011,10 +1041,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, /* * PPS keeps the existing slow Doppler path as a low-noise derivative * predictor for the position-WLS reference datum. This result is not - * published to fusion; fusion receives the raw robust solution above. + * published to fusion; fusion receives the adaptive low-latency + * solution above. */ for(unsigned int row = 0u; row < satellite_count; row++) { + velocity_measurements[row].cn0_dbhz = + measurements[row].cn0_dbhz; velocity_measurements[row].doppler_hz = measurements[row].filtered_doppler_hz; velocity_measurements[row].pseudorange_rate_mps = @@ -1037,6 +1070,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, &pps_velocity_excluded_row_mask); if(pps_velocity_valid) { + pps_velocity_speed_mps = + sqrt(pps_velocity_ecef_mps[0] * pps_velocity_ecef_mps[0] + + pps_velocity_ecef_mps[1] * pps_velocity_ecef_mps[1] + + pps_velocity_ecef_mps[2] * pps_velocity_ecef_mps[2]); pps_clock_bias_drift_valid = gps_pps_reference_bias_drift( solution.ecef_m, @@ -1050,8 +1087,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, (pps_velocity_excluded_row_mask != 0u)) { GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV, - "velocity FDE raw_mask=0x%02lx pps_mask=0x%02lx " - "raw_rms=%.3lf pps_rms=%.3lf\r\n", + "velocity FDE adaptive_mask=0x%02lx pps_mask=0x%02lx " + "adaptive_rms=%.3lf pps_rms=%.3lf\r\n", (unsigned long)solution.velocity_excluded_row_mask, (unsigned long)pps_velocity_excluded_row_mask, solution.velocity_residual_rms_mps, @@ -1178,11 +1215,21 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, } double filtered_clock_bias_m = 0.0; double clock_innovation_m = 0.0; + bool pps_predictor_enabled = + (GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) && + pps_clock_bias_drift_valid; +#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0 + pps_predictor_enabled = + (((gps_pps_predictor_ab_update_counter / + GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) & 1u) != 0u) && + pps_clock_bias_drift_valid; + gps_pps_predictor_ab_update_counter++; +#endif gps_pps_time_filter_result_t filter_result = gps_pps_filter_clock_bias( pps_reference_counter, solution.clock_bias_m, - pps_clock_bias_drift_valid, + pps_predictor_enabled, pps_clock_bias_drift_mps, solution.tdop, &filtered_clock_bias_m, @@ -1244,7 +1291,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, "PPS profile=%s mode=%s locked=%u tdop=%.2lf gdop=%.2lf " "clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm " "raw_bias=%.3lfm filtered_bias=%.3lfm " - "clock_drift=%.3lfmps bias_drift=%.3lfmps ref_svid=%u " + "clock_drift=%.3lfmps bias_drift=%.3lfmps " + "predictor=%u eskf_alpha=%.2lf " + "eskf_v=%.3lf/%.3lf/0x%02lx " + "pps_v=%.3lf/%.3lf/0x%02lx ref_svid=%u " "capture_corr=%.2lfns " "hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm " "pulse=%u seq=%u status=0x%03x\r\n", @@ -1261,6 +1311,14 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, filtered_clock_bias_m, solution.clock_drift_mps, pps_clock_bias_drift_mps, + pps_predictor_enabled ? 1u : 0u, + eskf_doppler_alpha, + solution.speed_mps, + solution.velocity_residual_rms_mps, + (unsigned long)solution.velocity_excluded_row_mask, + pps_velocity_speed_mps, + pps_velocity_residual_rms_mps, + (unsigned long)pps_velocity_excluded_row_mask, ephemeris[reference_index].SVID, pps_capture_correction_sec * 1.0e9, hardware_phase_ns, rate_command_ppm, integrator_ppm, diff --git a/soc/software/gps_system/src/gps/gps_receiver.c b/soc/software/gps_system/src/gps/gps_receiver.c index 725f819..62de205 100644 --- a/soc/software/gps_system/src/gps/gps_receiver.c +++ b/soc/software/gps_system/src/gps/gps_receiver.c @@ -42,3 +42,13 @@ uint64_t gps_receiver_status_ticks_per_second(void) { return GPS_SYSTICK_HZ; } + +void gps_receiver_set_eskf_doppler_alpha(double alpha) +{ + gps_tracking_set_eskf_doppler_alpha(alpha); +} + +double gps_receiver_get_eskf_doppler_alpha(void) +{ + return gps_tracking_get_eskf_doppler_alpha(); +} diff --git a/soc/software/gps_system/src/gps/gps_tracking_measurement.c b/soc/software/gps_system/src/gps/gps_tracking_measurement.c index a997bb7..eba53f6 100644 --- a/soc/software/gps_system/src/gps/gps_tracking_measurement.c +++ b/soc/software/gps_system/src/gps/gps_tracking_measurement.c @@ -141,6 +141,10 @@ bool gps_tracking_measurement_read( measurement_out->doppler_hz; measurement_out->filtered_pseudorange_rate_mps = measurement_out->pseudorange_rate_mps; + measurement_out->eskf_doppler_hz = + measurement_out->doppler_hz; + measurement_out->eskf_pseudorange_rate_mps = + measurement_out->pseudorange_rate_mps; unsigned int version = meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT; @@ -156,6 +160,8 @@ bool gps_tracking_measurement_read( cn0_valid && isfinite(measurement_out->doppler_hz) && isfinite(measurement_out->pseudorange_rate_mps) && + isfinite(measurement_out->eskf_doppler_hz) && + isfinite(measurement_out->eskf_pseudorange_rate_mps) && isfinite(measurement_out->filtered_doppler_hz) && isfinite(measurement_out->filtered_pseudorange_rate_mps); return measurement_out->valid; diff --git a/soc/software/gps_system/tests/test_gps_pps_time_filter.c b/soc/software/gps_system/tests/test_gps_pps_time_filter.c index d3577ba..1e65fcd 100644 --- a/soc/software/gps_system/tests/test_gps_pps_time_filter.c +++ b/soc/software/gps_system/tests/test_gps_pps_time_filter.c @@ -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"); diff --git a/soc/software/gps_system/tests/test_gps_tracking_measurement.c b/soc/software/gps_system/tests/test_gps_tracking_measurement.c index aa45c1a..6281078 100644 --- a/soc/software/gps_system/tests/test_gps_tracking_measurement.c +++ b/soc/software/gps_system/tests/test_gps_tracking_measurement.c @@ -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); diff --git a/soc/software/pps_python/README.md b/soc/software/pps_python/README.md index b65115f..1d7a372 100644 --- a/soc/software/pps_python/README.md +++ b/soc/software/pps_python/README.md @@ -125,6 +125,20 @@ grafik bu farkı açıkça gösterir. Raw `b/c` değerini PI phase error olarak doğrudan kullanmak doğru değildir. +Klasik GNSS denklemindeki `c*delta_t_r` clock bias ile buradaki `b` aynı datum +değildir: capture'ların ortak receiver zamanı önceden çıkarılmıştır. Doppler +velocity WLS'nin dördüncü state'i uydulara ortak receiver clock/frequency +drift'idir; fakat bu göreli `b` değerinin doğrudan türevi olarak kullanılmaz. +PPS modeli datumun değişimini seçilen referans uydunun geometrik range-rate'i, +broadcast satellite clock drift'i ve Doppler-WLS receiver velocity'sinden +hesaplar: + +```text +bias_drift = + -reference_geometric_range_rate + + c * reference_satellite_clock_drift +``` + ## İlk HOW ve PPS başlangıcı `parse_subframe()` HOW Z-count alanını `6 s` ile çarpıp `tow.sec` yapar. Bu, @@ -256,6 +270,16 @@ NaN, dört uydunun altına düşme, eşik sınırları ve histerezisi; `test_gps_pps_time_filter.c` drift prediction, gap/re-anchor ve hard-outlier reddini sınar. +2026-07-26 Genesys2 logu ile kalibre edilen aynı-datum A/B testinde position +WLS clock-bias gürültüsü `15.87 m`, referans-datum drift'i yaklaşık +`291.75 m/s`, Doppler drift gürültüsü `3.47 m/s` ve oscillator hatası +`1.592 ppm` olarak modele verildi. Linux C fixed-point testinde Doppler +predictor clock-bias RMS'i `2.75 m`, yalnız son bias'i tutan fallback ise +`21.22 m` verdi. Python modeli sekiz seed'de hem phase RMS hem 125 MHz grid'e +quantize edilmiş edge-jitter p95 için predictor'un fallback'ten iyi kalmasını +regresyon koşulu yapar. Kart status register phase'i fiziksel pin jitter'i +değildir; `15-20 ns` çıkış hedefi son olarak scope/TIC ile ölçülmelidir. + İki zaman eşiği bilinçli olarak ayrıdır: iki saniyelik watchdog gözlem yokken PPS validliğini holdover'a indirir; aynı referans SVID için Doppler-predicted clock-bias ve PI state'i 30 saniyeye kadar korunur. Modelde 15 saniyelik WLS diff --git a/soc/software/pps_python/test_pps_adaptive.py b/soc/software/pps_python/test_pps_adaptive.py index 9b6ad07..21196df 100644 --- a/soc/software/pps_python/test_pps_adaptive.py +++ b/soc/software/pps_python/test_pps_adaptive.py @@ -59,6 +59,90 @@ def noisy_observations( class AdaptivePpsTests(unittest.TestCase): + 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):