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
|
while the deterministic PI loop, fixed-point rate state and physical edge
|
||||||
generation remain in hardware.
|
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
|
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
|
shadow registers and must be followed by one `COMMIT` write. A new mailbox
|
||||||
must not be written while `STATUS.COMMIT_BUSY` is set.
|
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]
|
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
|
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
|
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.
|
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
|
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
|
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
|
ö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
|
entegre edilir. Ham, sabit `alpha=0.05` ve IMU hareket durumuna göre bant
|
||||||
`GPS_TRACK_DOPPLER_SMOOTH_ALPHA=0.05` çıkışı denenir. Bu ayrım önemlidir:
|
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
|
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
|
## 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
|
Doppler burst, common receiver clock-drift step, 15 saniye GPS kesintisi ve
|
||||||
birleşik arıza koşullarını içerir.
|
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
|
- 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.
|
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
|
- Common receiver clock-frequency hatası WLS'nin dördüncü clock-drift state'i
|
||||||
tarafından hızdan ayrılır.
|
tarafından hızdan ayrılır.
|
||||||
|
|
||||||
Bu sonuçlarla firmware'de ESKF için ham Doppler -> kanal validity/FDE ->
|
Genesys2 sabit-kart ölçümünde tam ham yolun velocity-WLS RMS'i `9.17 m/s`,
|
||||||
velocity WLS + covariance -> ESKF yolu uygulanmıştır. Ayrı yavaş-filtered
|
p95'i `16.71 m/s` ve FDE oranı `86/120` oldu. Aynı epoch'larda sabit
|
||||||
Doppler velocity yalnız PPS position-WLS clock-bias datumunun drift predictor'u
|
`alpha=0.05` yol `0.92 m/s` RMS, `1.63 m/s` p95 ve `0/120` FDE verdi. Bu
|
||||||
ve PPS kalite metriği için tutulur. Ayrıntılı tablo ve sınırlar
|
nedenle nominal modelde iyi görünen doğrudan ham yol production'a alınmadı.
|
||||||
`output/DOPPLER_ESKF_ANALYSIS.md` içindedir.
|
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
|
## Kurulum ve çalıştırma
|
||||||
|
|
||||||
|
|||||||
@@ -348,10 +348,10 @@ def write_report(path: Path, summaries: list[Summary], seed_count: int) -> None:
|
|||||||
"",
|
"",
|
||||||
"## Mimari kararı",
|
"## 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.",
|
"- 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.",
|
"- 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.",
|
||||||
"- 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.",
|
"- 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ı",
|
"## 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:
|
shared 100 Hz IMU stream:
|
||||||
|
|
||||||
* the legacy fixed-alpha Doppler IIR before 5 Hz velocity WLS;
|
* 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;
|
* a global IMU-driven adaptive-alpha IIR;
|
||||||
* raw Doppler with the per-satellite robust WLS gate used before the ESKF.
|
* 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ı
|
## 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.
|
- 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.
|
- 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.
|
||||||
- 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.
|
- 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ı
|
## Model sınırları
|
||||||
|
|
||||||
|
|||||||
@@ -2,6 +2,7 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import replace
|
||||||
import unittest
|
import unittest
|
||||||
|
|
||||||
import numpy as np
|
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))))
|
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):
|
class DopplerWlsTest(unittest.TestCase):
|
||||||
def test_four_state_wls_separates_velocity_and_clock_drift(self) -> None:
|
def test_four_state_wls_separates_velocity_and_clock_drift(self) -> None:
|
||||||
velocity = np.asarray([145.0, -32.0, 7.5])
|
velocity = np.asarray([145.0, -32.0, 7.5])
|
||||||
@@ -85,6 +99,56 @@ class AdaptiveAlphaTest(unittest.TestCase):
|
|||||||
|
|
||||||
|
|
||||||
class DopplerEskfIntegrationTest(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:
|
def test_eskf_filters_raw_doppler_without_pre_iir(self) -> None:
|
||||||
traces = run_doppler_eskf_scenario(
|
traces = run_doppler_eskf_scenario(
|
||||||
_scenario("steady_cruise"),
|
_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_NAV_SOLVE_TIMER_TICKS ?= 10
|
||||||
GPS_PPS_STATUS_LOG_INTERVAL ?= 10
|
GPS_PPS_STATUS_LOG_INTERVAL ?= 10
|
||||||
|
GPS_PPS_DOPPLER_PREDICTOR_ENABLE ?= 1
|
||||||
|
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES ?= 0
|
||||||
|
|
||||||
include ../common/app.mk
|
include ../common/app.mk
|
||||||
|
|
||||||
@@ -94,7 +96,9 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
|
|||||||
-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
|
-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
|
||||||
-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
|
-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
|
||||||
-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) \
|
-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
|
LDLIBS += -lm
|
||||||
|
|
||||||
ifeq ($(GPS_DEBUG),1)
|
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
|
build'ler arasinda profil adiyla kopyalanmalidir. Bu secenekler software
|
||||||
derleme parametresidir; mevcut bitstream ile `fpga-mcs` yeterlidir.
|
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:
|
Otomatik WHO_AM_I algilama varsayilandir:
|
||||||
|
|
||||||
```sh
|
```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
|
Ham 20 ms carrier snapshot'inda kartta gorulen birkac hertz jitter, L1'de
|
||||||
metre/saniye seviyesinde range-rate gurultusune ve zayif geometride daha buyuk
|
metre/saniye seviyesinde range-rate gurultusune ve zayif geometride daha buyuk
|
||||||
receiver velocity hatasina donusur. Software ISR her PRN icin ham degeri
|
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
|
```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
|
PPS yolu 50 Hz cadence'de yaklasik `0.39 s` zaman sabitine sahiptir. ESKF yolu
|
||||||
cadence'inde bu yaklasik `0.39 s` zaman sabitidir. Reacquisition/PRN
|
fusion motion state ve gravity-compensated linear acceleration ile bant
|
||||||
degisiminde eski kanal state'i kullanilmaz.
|
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,
|
IIR donanimda da uygulanabilirdi; bunun icin kanal basina state,
|
||||||
coefficient-multiply, rounding/saturation ve reacquisition reset kurali
|
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:
|
Her 200 ms navigation epoch'unda iki velocity tuketicisi vardir:
|
||||||
|
|
||||||
```text
|
```text
|
||||||
raw range-rate -> robust velocity WLS -> fusion adapter -> ESKF
|
adaptive range-rate -> robust velocity WLS -> fusion adapter -> ESKF
|
||||||
filtered range-rate -> robust velocity WLS -> PPS bias-drift predictor
|
slow range-rate -> robust velocity WLS -> PPS bias-drift predictor
|
||||||
```
|
```
|
||||||
|
|
||||||
Ham ESKF yolu temporal filtreleme isini ESKF'ye birakir ve `alpha=0.05`
|
Tam ham 20 ms snapshot yolu kart A/B'sinde fazla gurultulu bulundugu icin
|
||||||
IIR'in yaklasik 390 ms dinamik gecikmesini velocity measurement'a eklemez.
|
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
|
PPS'in ana olcumu position WLS `clock_bias_m` sonucudur. Filtreli velocity
|
||||||
yalniz bu referans-datum bias'inin 200 ms epoch'lar arasindaki turevini
|
yalniz bu referans-datum bias'inin 200 ms epoch'lar arasindaki turevini
|
||||||
ongormek ve PPS kalite profilini siniflandirmak icin kullanilir. Doppler
|
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.
|
Fusion adapter covariance'i `R_ecef_to_enu * P * R^T` ile ENU'ya dondurur.
|
||||||
2026-07-25 sabit kart verisinde zayif geometriyle gorulen
|
2026-07-25 sabit kart verisinde zayif geometriyle gorulen
|
||||||
`p95=6.020 m/s` speed hatasi nedeniyle default alt sinirlar yatay eksenlerde
|
`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
|
`2.0 m/s`, dikey eksende `3.0 m/s`'dir. Bu konservatif tabanlar zayif geometri
|
||||||
bu konservatif tabanlar temporal korelasyondan cok zayif geometri ve kartta
|
ve kartta kalan Doppler hata kuyruklarini sinirlar; formal
|
||||||
henuz yeniden kalibre edilmemis raw-Doppler hata kuyruklarini sinirlar; formal
|
|
||||||
covariance daha buyukse aynen korunur. Velocity innovation 3-DOF `%99.9`
|
covariance daha buyukse aynen korunur. Velocity innovation 3-DOF `%99.9`
|
||||||
`NIS=16.266` kapisindan gecer. Kabul/red sayilari ve son velocity NIS'i
|
`NIS=16.266` kapisindan gecer. Kabul/red sayilari ve son velocity NIS'i
|
||||||
`[FUSION]` UART status satirinda `velocity=accepted/rejected` ve `vel_nis`
|
`[FUSION]` UART status satirinda `velocity=accepted/rejected` ve `vel_nis`
|
||||||
@@ -964,9 +987,9 @@ GPS_LOG(level, category, ...)
|
|||||||
|
|
||||||
| `GPS_DEBUG_LEVEL` | Eklenen cikti |
|
| `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. |
|
| `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. |
|
| `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
|
`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
|
position WLS'deki pseudorange/code-phase hata ve zayif geometriyi tek basina
|
||||||
duzeltmiyor. Doppler velocity artik ESKF state update'ine baglidir; formal
|
duzeltmiyor. Doppler velocity artik ESKF state update'ine baglidir; formal
|
||||||
covariance, kart verisine dayali floor ve ayri NIS gate ile asiri guven
|
covariance, kart verisine dayali floor ve ayri NIS gate ile asiri guven
|
||||||
sinirlanir. Bu yeni software'in gercek kartta `vel_nis` ve
|
sinirlanir. Yukaridaki board istatistikleri ESKF entegrasyonundan onceki
|
||||||
`velocity=accepted/rejected` sayaclariyla saha A/B sonucu henuz olculmedi;
|
velocity dogrulamasidir; entegrasyon sonrasi `vel_nis` ve
|
||||||
yukaridaki board istatistikleri ESKF entegrasyonundan onceki ham velocity
|
`velocity=accepted/rejected` saha sonucu bir sonraki bolumdedir.
|
||||||
dogrulamasidir.
|
|
||||||
|
### 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
|
### 2026-07-08 ACQ Debug Notu
|
||||||
|
|
||||||
|
|||||||
@@ -29,6 +29,22 @@
|
|||||||
#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
|
#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
|
||||||
#define IMU_SAMPLES_AFTER_GPS_SERVICE 3u
|
#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)
|
static void initialize_sensor_fusion(gps_system_state_t* system)
|
||||||
{
|
{
|
||||||
system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
|
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))
|
imu_sensor_take_measurement(&system->imu_sample))
|
||||||
{
|
{
|
||||||
bool accepted = navigation_fusion_push_imu(&system->imu_sample);
|
bool accepted = navigation_fusion_push_imu(&system->imu_sample);
|
||||||
if(accepted &&
|
bool output_available = accepted &&
|
||||||
navigation_fusion_get_output(&system->fused_solution) &&
|
navigation_fusion_get_output(&system->fused_solution);
|
||||||
system->fused_solution.global_valid &&
|
if(output_available)
|
||||||
|
{
|
||||||
|
update_eskf_doppler_bandwidth(&system->fused_solution);
|
||||||
|
}
|
||||||
|
if(output_available && system->fused_solution.global_valid &&
|
||||||
(system->fused_solution.timestamp_ticks !=
|
(system->fused_solution.timestamp_ticks !=
|
||||||
system->last_fused_location_tick))
|
system->last_fused_location_tick))
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -19,6 +19,7 @@ typedef struct {
|
|||||||
double height_m;
|
double height_m;
|
||||||
double position_std_m[3];
|
double position_std_m[3];
|
||||||
double speed_mps;
|
double speed_mps;
|
||||||
|
double linear_acceleration_norm_mps2;
|
||||||
double last_position_nis;
|
double last_position_nis;
|
||||||
double last_velocity_nis;
|
double last_velocity_nis;
|
||||||
fusion_motion_state_t motion_state;
|
fusion_motion_state_t motion_state;
|
||||||
|
|||||||
@@ -55,6 +55,8 @@
|
|||||||
void gps_background_task(void);
|
void gps_background_task(void);
|
||||||
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
|
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
|
||||||
void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
|
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);
|
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(void);
|
||||||
uint64_t gps_receiver_status_ticks_per_second(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
|
#endif
|
||||||
|
|||||||
@@ -6,6 +6,8 @@
|
|||||||
|
|
||||||
#include "gps/gps_ip_defines.h"
|
#include "gps/gps_ip_defines.h"
|
||||||
|
|
||||||
|
#define GPS_TRACK_PPS_DOPPLER_ALPHA 0.05
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
uint32_t prn;
|
uint32_t prn;
|
||||||
uint32_t sequence;
|
uint32_t sequence;
|
||||||
@@ -17,9 +19,12 @@ typedef struct {
|
|||||||
int32_t cn0_sum_q;
|
int32_t cn0_sum_q;
|
||||||
uint64_t cn0_wbp;
|
uint64_t cn0_wbp;
|
||||||
unsigned int cn0_sample_count;
|
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 doppler_hz;
|
||||||
double pseudorange_rate_mps;
|
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. */
|
/* Slow software IIR path retained only for PPS bias-drift prediction. */
|
||||||
double filtered_doppler_hz;
|
double filtered_doppler_hz;
|
||||||
double filtered_pseudorange_rate_mps;
|
double filtered_pseudorange_rate_mps;
|
||||||
|
|||||||
@@ -115,7 +115,7 @@ void gps_system_uart_publish_periodic_fusion_status(
|
|||||||
|
|
||||||
if(output->global_valid)
|
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 "
|
"state=%s lat=%.10lf lon=%.10lf height=%.3lf "
|
||||||
"speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf "
|
"speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf "
|
||||||
"gps=%lu/%lu velocity=%lu/%lu recoveries=%lu "
|
"gps=%lu/%lu velocity=%lu/%lu recoveries=%lu "
|
||||||
@@ -142,7 +142,7 @@ void gps_system_uart_publish_periodic_fusion_status(
|
|||||||
return;
|
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 "
|
"state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
|
||||||
"imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
|
"imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
|
||||||
navigation_fusion_state_name(output->engine_state),
|
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]));
|
sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
|
||||||
}
|
}
|
||||||
output->speed_mps = eskf_speed_mps(&runtime->filter);
|
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_position_nis = runtime->filter.last_position_nis;
|
||||||
output->last_velocity_nis = runtime->filter.last_velocity_nis;
|
output->last_velocity_nis = runtime->filter.last_velocity_nis;
|
||||||
output->motion_state = runtime->motion.state;
|
output->motion_state = runtime->motion.state;
|
||||||
|
|||||||
@@ -9,6 +9,7 @@
|
|||||||
#include <bsp/irq.h>
|
#include <bsp/irq.h>
|
||||||
#include <bsp/mmio.h>
|
#include <bsp/mmio.h>
|
||||||
#include <bsp/plic.h>
|
#include <bsp/plic.h>
|
||||||
|
#include <math.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
typedef enum {
|
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)
|
(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u)
|
||||||
#define GPS_NAV_HOLDOFF_LOCAL_TICKS \
|
#define GPS_NAV_HOLDOFF_LOCAL_TICKS \
|
||||||
(((uint64_t)BSP_CLINT_TIMEBASE_HZ * GPS_NAV_EPOCH_HOLDOFF_MS) / 1000u)
|
(((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
|
#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 <
|
_Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS <
|
||||||
(GPS_NAV_SOLVE_PERIOD_MS / 2u),
|
(GPS_NAV_SOLVE_PERIOD_MS / 2u),
|
||||||
"NAV capture window must be below half the solve period");
|
"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;
|
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)
|
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);
|
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.
|
* 0.4 s time constant while retaining real receiver dynamics.
|
||||||
*/
|
*/
|
||||||
measurement.filtered_doppler_hz =
|
measurement.filtered_doppler_hz =
|
||||||
GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz +
|
GPS_TRACK_PPS_DOPPLER_ALPHA * measurement.doppler_hz +
|
||||||
(1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) *
|
(1.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) *
|
||||||
previous->filtered_doppler_hz;
|
previous->filtered_doppler_hz;
|
||||||
measurement.filtered_pseudorange_rate_mps =
|
measurement.filtered_pseudorange_rate_mps =
|
||||||
gps_tracking_doppler_to_range_rate_mps(
|
gps_tracking_doppler_to_range_rate_mps(
|
||||||
measurement.filtered_doppler_hz);
|
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;
|
measurement.local_timestamp_ticks = local_timestamp_ticks;
|
||||||
gps_latest_tracking_measurement[tracking_index] = measurement;
|
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 unsigned int gps_pps_reference_svid;
|
||||||
static bool gps_pps_filter_reference_svid_valid;
|
static bool gps_pps_filter_reference_svid_valid;
|
||||||
static unsigned int gps_pps_filter_reference_svid;
|
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)
|
#if !defined(GPS_PPS_STATUS_LOG_INTERVAL) || (GPS_PPS_STATUS_LOG_INTERVAL < 1)
|
||||||
#error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one"
|
#error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one"
|
||||||
#endif
|
#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;
|
volatile uint64_t gps_systick_ticks = 0;
|
||||||
|
|
||||||
static void gps_init_acq_tables(void)
|
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;
|
bool pps_clock_bias_drift_valid = false;
|
||||||
double pps_clock_bias_drift_mps = 0.0;
|
double pps_clock_bias_drift_mps = 0.0;
|
||||||
bool pps_velocity_valid = false;
|
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;
|
double pps_velocity_residual_rms_mps = 0.0;
|
||||||
uint32_t pps_velocity_excluded_row_mask = 0u;
|
uint32_t pps_velocity_excluded_row_mask = 0u;
|
||||||
gps_nav_solution_t solution;
|
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)
|
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_positions_array[GPS_TRCK_IP_COUNT][3];
|
||||||
double satellite_velocities_array[GPS_TRCK_IP_COUNT][3];
|
double satellite_velocities_array[GPS_TRCK_IP_COUNT][3];
|
||||||
gps_tracking_measurement_t
|
gps_tracking_measurement_t
|
||||||
@@ -978,6 +1002,12 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
|||||||
velocities->matrix[row][component];
|
velocities->matrix[row][component];
|
||||||
}
|
}
|
||||||
velocity_measurements[row] = measurements[row];
|
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)
|
if((solution.used_row_mask & (1u << row)) == 0u)
|
||||||
{
|
{
|
||||||
velocity_measurements[row].valid = false;
|
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
|
* PPS keeps the existing slow Doppler path as a low-noise derivative
|
||||||
* predictor for the position-WLS reference datum. This result is not
|
* 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++)
|
for(unsigned int row = 0u; row < satellite_count; row++)
|
||||||
{
|
{
|
||||||
|
velocity_measurements[row].cn0_dbhz =
|
||||||
|
measurements[row].cn0_dbhz;
|
||||||
velocity_measurements[row].doppler_hz =
|
velocity_measurements[row].doppler_hz =
|
||||||
measurements[row].filtered_doppler_hz;
|
measurements[row].filtered_doppler_hz;
|
||||||
velocity_measurements[row].pseudorange_rate_mps =
|
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);
|
&pps_velocity_excluded_row_mask);
|
||||||
if(pps_velocity_valid)
|
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 =
|
pps_clock_bias_drift_valid =
|
||||||
gps_pps_reference_bias_drift(
|
gps_pps_reference_bias_drift(
|
||||||
solution.ecef_m,
|
solution.ecef_m,
|
||||||
@@ -1050,8 +1087,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
|||||||
(pps_velocity_excluded_row_mask != 0u))
|
(pps_velocity_excluded_row_mask != 0u))
|
||||||
{
|
{
|
||||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
|
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
|
||||||
"velocity FDE raw_mask=0x%02lx pps_mask=0x%02lx "
|
"velocity FDE adaptive_mask=0x%02lx pps_mask=0x%02lx "
|
||||||
"raw_rms=%.3lf pps_rms=%.3lf\r\n",
|
"adaptive_rms=%.3lf pps_rms=%.3lf\r\n",
|
||||||
(unsigned long)solution.velocity_excluded_row_mask,
|
(unsigned long)solution.velocity_excluded_row_mask,
|
||||||
(unsigned long)pps_velocity_excluded_row_mask,
|
(unsigned long)pps_velocity_excluded_row_mask,
|
||||||
solution.velocity_residual_rms_mps,
|
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 filtered_clock_bias_m = 0.0;
|
||||||
double clock_innovation_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_time_filter_result_t filter_result =
|
||||||
gps_pps_filter_clock_bias(
|
gps_pps_filter_clock_bias(
|
||||||
pps_reference_counter,
|
pps_reference_counter,
|
||||||
solution.clock_bias_m,
|
solution.clock_bias_m,
|
||||||
pps_clock_bias_drift_valid,
|
pps_predictor_enabled,
|
||||||
pps_clock_bias_drift_mps,
|
pps_clock_bias_drift_mps,
|
||||||
solution.tdop,
|
solution.tdop,
|
||||||
&filtered_clock_bias_m,
|
&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 "
|
"PPS profile=%s mode=%s locked=%u tdop=%.2lf gdop=%.2lf "
|
||||||
"clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm "
|
"clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm "
|
||||||
"raw_bias=%.3lfm filtered_bias=%.3lfm "
|
"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 "
|
"capture_corr=%.2lfns "
|
||||||
"hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm "
|
"hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm "
|
||||||
"pulse=%u seq=%u status=0x%03x\r\n",
|
"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,
|
filtered_clock_bias_m,
|
||||||
solution.clock_drift_mps,
|
solution.clock_drift_mps,
|
||||||
pps_clock_bias_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,
|
ephemeris[reference_index].SVID,
|
||||||
pps_capture_correction_sec * 1.0e9,
|
pps_capture_correction_sec * 1.0e9,
|
||||||
hardware_phase_ns, rate_command_ppm, integrator_ppm,
|
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;
|
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->doppler_hz;
|
||||||
measurement_out->filtered_pseudorange_rate_mps =
|
measurement_out->filtered_pseudorange_rate_mps =
|
||||||
measurement_out->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 =
|
unsigned int version =
|
||||||
meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT;
|
meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT;
|
||||||
@@ -156,6 +160,8 @@ bool gps_tracking_measurement_read(
|
|||||||
cn0_valid &&
|
cn0_valid &&
|
||||||
isfinite(measurement_out->doppler_hz) &&
|
isfinite(measurement_out->doppler_hz) &&
|
||||||
isfinite(measurement_out->pseudorange_rate_mps) &&
|
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_doppler_hz) &&
|
||||||
isfinite(measurement_out->filtered_pseudorange_rate_mps);
|
isfinite(measurement_out->filtered_pseudorange_rate_mps);
|
||||||
return measurement_out->valid;
|
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);
|
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)
|
static void test_reference_geometry_defines_bias_drift(void)
|
||||||
{
|
{
|
||||||
const double receiver_position_m[3] = {0.0, 0.0, 0.0};
|
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_drift_prediction_rejects_bias_noise();
|
||||||
test_quality_gap_and_outlier_handling();
|
test_quality_gap_and_outlier_handling();
|
||||||
test_bias_ramp_without_doppler_uses_bounded_fallback();
|
test_bias_ramp_without_doppler_uses_bounded_fallback();
|
||||||
|
test_board_calibrated_doppler_predictor_beats_fallback();
|
||||||
test_reference_geometry_defines_bias_drift();
|
test_reference_geometry_defines_bias_drift();
|
||||||
test_reference_selection_is_stable();
|
test_reference_selection_is_stable();
|
||||||
puts("gps_pps_time_filter tests passed");
|
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.filtered_pseudorange_rate_mps,
|
||||||
measurement.pseudorange_rate_mps,
|
measurement.pseudorange_rate_mps,
|
||||||
1.0e-12));
|
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 &=
|
registers.MEASUREMENT_META &=
|
||||||
~(1u << GPS_TRCK_MEAS_META_LOCK_POS);
|
~(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.
|
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ı
|
## İlk HOW ve PPS başlangıcı
|
||||||
|
|
||||||
`parse_subframe()` HOW Z-count alanını `6 s` ile çarpıp `tow.sec` yapar. Bu,
|
`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
|
`test_gps_pps_time_filter.c` drift prediction, gap/re-anchor ve hard-outlier
|
||||||
reddini sınar.
|
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
|
İ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
|
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
|
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):
|
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):
|
def test_quality_hysteresis_rejects_metric_chatter(self):
|
||||||
selector = QualitySelector()
|
selector = QualitySelector()
|
||||||
for _ in range(7):
|
for _ in range(7):
|
||||||
|
|||||||
Reference in New Issue
Block a user