fix(gps): adapt Doppler bandwidth from board validation
This commit is contained in:
+22
@@ -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.
|
||||
|
||||
+27
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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ı",
|
||||
"",
|
||||
|
||||
@@ -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.
|
||||
|
||||
|
||||
@@ -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ı
|
||||
|
||||
|
||||
@@ -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"),
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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))
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
#include <bsp/irq.h>
|
||||
#include <bsp/mmio.h>
|
||||
#include <bsp/plic.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
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;
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
Reference in New Issue
Block a user