diff --git a/README.md b/README.md index 023aedc..64fd25a 100644 --- a/README.md +++ b/README.md @@ -136,7 +136,7 @@ tracking loop, her 20 ms: -> sabit tracking snapshot -> synchronizer + APB-domain atomik register + interrupt -> C/N0 ve Doppler/range-rate donusumu - -> satellite velocity/clock-drift duzeltmeli C/N0-weighted velocity WLS + -> ham Doppler + tek-kanal FDE ile C/N0-weighted velocity WLS -> ECEF velocity + 3x3 covariance + receiver clock drift -> ECEF->ENU adapter -> ayri velocity NIS gate @@ -145,14 +145,42 @@ tracking loop, her 20 ms: ``` RTL Doppler veya C/N0 uzerinde IIR yapmaz; ham ve 20 ms sabit snapshot'i -software'e verir. Software ayni PRN icin C/N0'yi `alpha=0.2`, Doppler'i -`alpha=0.05` ile yumusatir. L1 range-rate -`-doppler_hz * c / 1575420000` ile hesaplanir. En az dort uyduyla cozulmus -velocity WLS state'i `[vx, vy, vz, receiver_clock_drift_mps]`'dir; uydu LOS -hizi ve broadcast satellite clock drift observation'dan ayrilir. C/N0, +software'e verir. Software ayni PRN icin C/N0'yi `alpha=0.2` ile yumusatir; +Doppler'in hem ham de `alpha=0.05` IIR sonucu korunur. L1 range-rate +`-doppler_hz * c / 1575420000` ile hesaplanir. Ham yol tek-kanal +leave-one-out FDE'li velocity WLS'ten ESKF'ye, filtreli yol ayri velocity +WLS'ten yalniz PPS clock-bias drift predictor'una gider. PPS'in ana faz/zaman +olcumu position WLS `clock_bias_m` sonucudur; Doppler yalniz 200 ms epoch'lar +arasindaki turev prediction'ina ve kalite siniflandirmasina yardim eder. Her +iki velocity WLS state'i `[vx, vy, vz, receiver_clock_drift_mps]`'dir; uydu +LOS hizi ve broadcast satellite clock drift observation'dan ayrilir. C/N0, range-rate agirligini belirler; post-fit residual gerekirse formal covariance'i yalniz buyutur. +Klasik mutlak-pseudorange GNSS modelinde receiver zaman hatasi +`delta_t_r(t)` ise: + +```text +code WLS 4. state: b_r = c * delta_t_r [metre] +Doppler WLS 4. state: bdot_r = c * d(delta_t_r)/dt [metre/saniye] +b_r(t + dt) ~= b_r(t) + bdot_r * dt +``` + +Burada `bdot_r`, receiver oscillator frequency error'inin range-rate +birimindeki ortak etkisidir. Ornegin `0.1 ppm` yaklasik `29.98 m/s` eder; +Doppler WLS bu ortak terimi uydu yonune gore degisen gercek `vx/vy/vz` +hareketinden ayirir. + +Bu projede onemli bir datum farki vardir: position WLS'e verilen capture +zamanlarindan ortak receiver zamani cikarilir ve `STARTOFFSET` eklenir. Bu +nedenle `solution.clock_bias_m`, saf `c*delta_t_r` degil, yaklasik +`c*STARTOFFSET - reference_range` olan referans-uyduya bagli goreli +datumdur. Doppler WLS'nin dorduncu bilinmeyeni yine uydulara ortak receiver +clock/frequency drift'tir; fakat iki fit arasinda +`clock_bias_m(t+dt)=clock_bias_m(t)+clock_drift_mps*dt` kisiti kurulmaz. +Fit'ler ayri measurement/normal-equation kullanir. Doppler WLS, LOS +vektorleri icin position WLS'nin guncel receiver konumuna baglidir. + ESKF; ENU position/velocity, quaternion attitude, accel bias ve gyro bias iceren 15 hata durumlu filtredir. Her timestamp'li IMU ornegi prediction'a girer. Gecikmis GPS position ve Doppler velocity olcumleri kendi capture diff --git a/soc/software/fusion/README.md b/soc/software/fusion/README.md index a1bfc32..6648307 100644 --- a/soc/software/fusion/README.md +++ b/soc/software/fusion/README.md @@ -98,14 +98,17 @@ TDK veya ST IMU -> imu_sensor ----/ kontrol eder. Carrier değeri için `doppler_hz = carrier_q7/128 - 4092000`, L1 pseudorange-rate için `range_rate = -doppler_hz*c/1575420000` kullanılır. -3. Aynı PRN'in 50 Hz snapshot'larında C/N0 `alpha=0.2`, Doppler `alpha=0.05` - software IIR ile yumuşatılır. RTL ham ölçümü korur; hardware Doppler IIR - içermez. -4. En az dört uyduyla C/N0-weighted velocity WLS +3. Aynı PRN'in 50 Hz snapshot'larında C/N0 `alpha=0.2` ile yumuşatılır. + Doppler'in ham değeri ile `alpha=0.05` software-IIR değeri ayrı tutulur; + RTL hardware Doppler IIR içermez. +4. Ham Doppler, en az dört uyduyla C/N0-weighted velocity WLS `[receiver_vx, receiver_vy, receiver_vz, receiver_clock_drift_mps]` state'ini çözer. Her observation'dan satellite LOS velocity çıkarılır, broadcast satellite clock drift eklenir. Inverse normal matrix ve post-fit - residual ECEF velocity covariance'ini üretir. + residual ECEF velocity covariance'ini üretir. En az altı kullanılabilir + uyduda `3.5 sigma` normalized-residual gate açılırsa bütün tek-kanal + leave-one-out adayları denenir ve en düşük weighted residual'lı aday ESKF + yolu için seçilir. 5. GPS adapter velocity ve 3x3 covariance'i ECEF'ten ENU'ya döndürür, kart ölçümüne dayalı minimum `2/2/3 m/s` standard-deviation floor'larını uygular ve position ile aynı capture timestamp'ini korur. @@ -114,6 +117,28 @@ TDK veya ST IMU -> imu_sensor ----/ velocity reject aynı epoch'taki kabul edilmiş position update'ini bozmaz. Sonra daha yeni IMU history current-time horizon'a kadar replay edilir. +Filtreli Doppler ikinci bir velocity WLS'e girer; bu sonuç fusion/ESKF'ye +yayınlanmaz. Yalnız PPS yazılım filtresinde position-WLS referans datumunun +clock-bias drift predictor'u ve PPS kalite metriği olarak kullanılır. PPS'in +ana ölçümü position WLS `clock_bias_m` değeridir. Doppler WLS'nin dördüncü +`receiver_clock_drift_mps` state'i PPS'e doğrudan verilmez; filtreli receiver +velocity ve referans uydu ephemeris hızıyla datum türevi ayrıca hesaplanır. + +Klasik mutlak-pseudorange modelinde receiver clock bias +`c*delta_t_r`, Doppler clock drift ise bunun türevi +`c*d(delta_t_r)/dt`'dir. Doppler WLS carrier pseudorange-rate'ten +`[vx,vy,vz,receiver_clock_drift]` çözer. `0.1 ppm` receiver oscillator +error'i yaklaşık `29.98 m/s` ortak Doppler drift'idir; dördüncü state +çözülmezse bunun bir bölümü platform hızı sanılır. + +Bu projedeki position WLS bias'i ise capture'lar ortak receiver zamanına göre +farklandığı ve `STARTOFFSET` eklendiği için saf receiver clock bias'i değil, +referans-uyduya bağlı göreli bir datumdur. Bu yüzden position +`clock_bias_m` ile Doppler `clock_drift_mps` arasında doğrudan türev kısıtı +kurulmaz. Doppler WLS ayrı bir normal equation kullanır fakat LOS vektörleri +için güncel position WLS receiver konumuna, uydu konum/hız/clock drift'i için +broadcast ephemerise bağlıdır. + Bu zincirin RTL/register ayrıntısı `hardware/external/gps/rtl/trck_src/README.md`, bare-metal WLS ve kart doğrulaması `soc/software/gps_system/README.md` içindedir. diff --git a/soc/software/gps_measurement_python/.gitignore b/soc/software/gps_measurement_python/.gitignore new file mode 100644 index 0000000..98dbeb4 --- /dev/null +++ b/soc/software/gps_measurement_python/.gitignore @@ -0,0 +1,3 @@ +__pycache__/ +*.py[cod] +.venv/ diff --git a/soc/software/gps_measurement_python/README.md b/soc/software/gps_measurement_python/README.md new file mode 100644 index 0000000..09f85e0 --- /dev/null +++ b/soc/software/gps_measurement_python/README.md @@ -0,0 +1,226 @@ +# GPS code measurement ve DLL kalite modeli + +Bu dizin, mevcut firmware/RTL ölçüm yolunu değiştirmeden önce code-phase +hassasiyeti, DLL jitter telemetrisi, dinamik WLS sigma ve carrier/Doppler +smoothing seçeneklerini aynı sentetik veri üzerinde karşılaştırır. + +Bu genel amaçlı bir RF/GNSS simülatörü değildir. Projeye özgü executable +tasarım modelidir. + +Dizin iki ayrı kararı inceler: + +1. Position WLS için C/N0'ın açıklamadığı DLL jitter/variance telemetrisi. +2. Velocity WLS ve ESKF öncesindeki sabit Doppler IIR'in gürültü-gecikme + etkisi. + +## Modellenen mevcut mimari + +| Proje davranışı | Model | +|---|---| +| Normalized early-minus-late DLL discriminator | `rtl_dll_model.rtl_discriminator()` | +| Signed Q2.13 discriminator quantization | `rtl_dll_model.py` | +| `LF_code.sv` C1=`21.25`, C2=`0.05712890625` ve Q20 çıkış | `CodeLoopFilterRtl` | +| 20 adet 1 ms DLL örneği/NAV bit | `synthesize_dll_window()` | +| Firmware C/N0 IIR, alpha=`0.2` | `ChannelQuality.update()` | +| URA/elevation/C/N0 position sigma | `firmware_sigma_m(CURRENT)` | +| ECEF x/y/z + common clock-bias WLS | `firmware_wls()` | +| 99% chi-square ve en az 7 uyduda leave-one-out FDE | `firmware_wls()` | +| Position solve periyodu 200 ms | `measurement_scenarios.py` | +| Doppler IIR alpha=`0.05`, 50 Hz | `doppler_eskf_model.py` | +| ECEF velocity + common clock-drift WLS | `velocity_wls()` | +| Velocity WLS periyodu 200 ms | `doppler_eskf_model.py` | +| TDK 100 Hz IMU process-noise profili | `VelocityBiasEskf` | +| Velocity 3-DOF `%99.9`, NIS=`16.266236` | `VelocityBiasEskf.update_velocity()` | + +Kaynak eşlemeleri: + +- `hardware/external/gps/rtl/trck_src/code_loop_discriminator.sv` +- `hardware/external/gps/rtl/trck_src/LF_code.sv` +- `hardware/external/gps/rtl/trck_src/NCO_code.sv` +- `hardware/external/gps/rtl/trck_src/tracking_measurement.sv` +- `hardware/external/gps/rtl/common/definitions.sv` +- `soc/software/gps_system/src/gps/gps_tracking_measurement.c` +- `soc/software/gps_system/src/gps/gps_interrupt_handlers.c` +- `soc/software/gps_system/src/gps/NavEquations.c` + +`REM_CODE_PHASE`, DLL discriminator residual'ı olarak modellenmez. RTL'de bu +değer code NCO'nun epoch taşma remainder'ıdır. Yeni kalite metriğinin girdisi +RTL içindeki gerçek signed `dll_error` örnekleridir. + +## Karşılaştırılan sigma politikaları + +- `current_cn0`: mevcut URA + elevation + C/N0 modeli. +- `naive_add_dll`: DLL variance'ı bağımsızmış gibi ekler. Nominal durumda aynı + thermal jitter'ı iki kere saydığı için negatif kontrol olarak bulunur. +- `dll_excess`: yalnız ölçülen DLL variance'ın C/N0'dan beklenen variance'ı aşan + bölümünü code sigma'ya ekler. +- `dll_excess_guard`: excess variance'a ek olarak sürekli discriminator mean + stress'inde ağırlığı azaltır. +- `oracle_variance`: sentetik gerçek jitter çarpanını bilir. Ulaşılabilir üst + sınırı gösterir; firmware algoritması değildir. + +Dinamik model: + +```text +sigma_base^2 = sigma_URA^2 + sigma_elevation^2 + sigma_CN0^2 +ratio = EWMA(measured_DLL_variance / expected_DLL_variance(C/N0)) + +sigma_dynamic^2 = + sigma_URA^2 + + sigma_elevation^2 + + sigma_CN0^2 * max(1, ratio) +``` + +Bu form C/N0 ile DLL thermal jitter'ını iki kere toplamaz. Yalnızca C/N0 +modelinin açıklayamadığı kanal-bağımlı excess jitter'ı şişirir. + +## Senaryolar + +- Temiz açık gökyüzü +- Bütün kanallarda düşük C/N0 +- C/N0'da görünmeyen tek-kanal DLL jitter +- Bütün kanallarda ortak, C/N0'da görünmeyen DLL jitter +- Gürültü burst ve recovery +- Re-lock sırasında DLL mean/jitter stress +- Düşük-variance stable multipath bias +- Bias + excess jitter +- Kötü geometri + gizli jitter +- İlk başta düşük, sonra iyileşen C/N0 + +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: +PPS datum-drift predictor'u için yararlı olan yavaş Doppler filtresi, ESKF +velocity veya hızlı code-aiding prediction'ında dinamik lag üretebilir. + +## Doppler pre-IIR ve ESKF modeli + +`doppler_eskf_model.py`, aynı truth, tracking gürültüsü ve IMU örnekleriyle şu +dört yolu karşılaştırır: + +- Önceki: 50 Hz `alpha=0.05` IIR -> 5 Hz velocity WLS -> ESKF. +- Ham Doppler -> 5 Hz velocity WLS -> ESKF. +- C/N0 + düşük-geçiren navigation acceleration ile adaptif IIR -> WLS -> ESKF. +- Ham Doppler -> RAIM-benzeri leave-one-out robust WLS -> ESKF. + +Mevcut `alpha=0.05` tek-kutuplu IIR için 50 Hz'de eşdeğer cutoff yaklaşık +`0.408 Hz`, zaman sabiti `0.390 s`, DC group delay ise yaklaşık `380 ms`'dir. +Bu gecikme constant velocity'de görünmez; acceleration sırasında velocity +measurement innovation'ına doğrudan girer. + +ESKF modeli bu karar için gerekli `[velocity, accel-bias]` alt sistemidir. +Firmware'deki TDK process acceleration noise density `0.025 +m/s2/sqrt(Hz)`, accelerometer bias random walk `0.0015 m/s2/sqrt(s)`, WLS'nin +gerçek 3x3 covariance'ı, fusion adapter'ın `2/2/3 m/s` ENU velocity sigma +tabanları ve velocity NIS kapısı kullanılır. Position, attitude, lever-arm ve +tam 15-state cross-covariance bu modelin kapsamında değildir. + +Senaryo matrisi sabit uçuş, airliner/aggressive manevra, düşük C/N0, tek-kanal +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: + +- 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. +- Sabit pre-IIR sakin uçuşta `0.182 m/s`, ham yol `0.190 m/s` verir. Airliner + manevrasında sabit IIR gecikmesi sonucu `0.416 m/s`, ham yol `0.190 m/s`; + aggressive manevrada sırasıyla `0.942 m/s` ve `0.190 m/s` oluşur. Adapter + sigma tabanları iki yolu da NIS içinde tutsa bile dinamik lag kalır. +- Düşük C/N0 manevrasında adaptif taslak en iyi gürültü/gecikme uzlaşmasını + verir; eşikleri firmware'e taşımadan önce kart loguyla kalibre etmek gerekir. +- ESKF yalnız aggregate 3D velocity gördüğünden tek uydu burst'ünün kaynağını + ayıramaz. Kanal outlier/FDE işlemi velocity WLS öncesinde yapılmalıdır. +- 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. + +## Kurulum ve çalıştırma + +Repo içindeki `soc/software` dizininden: + +```bash +python3 -m venv gps_measurement_python/.venv +gps_measurement_python/.venv/bin/pip install \ + -r gps_measurement_python/requirements.txt +gps_measurement_python/.venv/bin/python -m unittest \ + gps_measurement_python/test_measurement_model.py \ + gps_measurement_python/test_doppler_eskf_model.py +gps_measurement_python/.venv/bin/python \ + -m gps_measurement_python.analyze_measurement_scenarios \ + --seeds 12 --jobs 4 +gps_measurement_python/.venv/bin/python \ + -m gps_measurement_python.analyze_doppler_eskf \ + --seeds 20 --jobs 4 +``` + +Sistem Python'unda `venv` paketi yoksa bağımlılıklar izole bir target dizinine +de kurulabilir: + +```bash +python3 -m pip install --target /tmp/gps-measurement-deps \ + -r gps_measurement_python/requirements.txt +PYTHONPATH=/tmp/gps-measurement-deps:. python3 \ + -m unittest gps_measurement_python/test_measurement_model.py +PYTHONPATH=/tmp/gps-measurement-deps:. python3 \ + -m gps_measurement_python.analyze_measurement_scenarios \ + --seeds 12 --jobs 4 +``` + +Çıktılar `gps_measurement_python/output/` altındadır: + +- `scenario_summary.csv` +- `ANALYSIS.md` +- `01_position_3d_rms.png` +- `02_solution_availability.png` +- `03_noise_burst_transition.png` +- `04_code_smoothing_cfo.png` +- `doppler_eskf_summary.csv` +- `DOPPLER_ESKF_ANALYSIS.md` +- `05_doppler_eskf_velocity_rms.png` +- `06_airliner_maneuver_trace.png` +- `07_outlier_wls_nis.png` +- `08_low_cn0_wls_vs_eskf.png` + +## Kalibrasyon sınırı + +Modelde C/N0'dan beklenen normalized discriminator standard deviation'ı +45 dB-Hz için `0.020` kabul edilir ve firmware ile aynı 20 dB amplitude yasası +ile ölçeklenir. Bu sayı karttan alınmış bir kalibrasyon değildir. + +Karar vermeden önce karttan her kanal için en az şu 50 Hz kayıt alınmalıdır: + +```text +sequence, PRN, C/N0, +DLL mean, DLL variance, DLL sample count, +code NCO correction, carrier frequency, +ABS_SAMPLE, REM_CODE_PHASE, lock +``` + +Kayıtlarla `expected_DLL_variance(C/N0)` eğrisi çıkarılmalı; temperature, +elevation ve lock yaşına göre ayrıştırılmalıdır. Stable multipath, DLL yeni +denge noktasına oturduğunda düşük variance bırakabilir. Bu durum tek başına +DLL variance ile çözülemez; ek correlator tap'leri veya correlation-shape +metriği gerekir. + +RTL için en düşük maliyetli keşif arayüzü, her 20 ms NAV-bit penceresinde +`dll_error` üzerinden aşağıdaki ham istatistikleri biriktirmektir: + +```text +signed sum(error) : 15 + ceil(log2(20)) = 20 bit +unsigned sum(error^2) : 30 + ceil(log2(20)) = 35 bit +valid sample count : 5 bit +``` + +35-bit kare toplamı APB'de iki 32-bit register ile taşınabilir. Ortalama, +sample variance, C/N0 expectation ratio ve hysteresis yazılımda hesaplanır. +Bölme ve square-root RTL'ye konmaz. Bu tuple mevcut measurement sequence, +C/N0, carrier frequency, `ABS_SAMPLE` ve `REM_CODE_PHASE` ile aynı 20 ms +snapshot'ta tutulmalıdır. diff --git a/soc/software/gps_measurement_python/__init__.py b/soc/software/gps_measurement_python/__init__.py new file mode 100644 index 0000000..57dd220 --- /dev/null +++ b/soc/software/gps_measurement_python/__init__.py @@ -0,0 +1 @@ +"""Project-specific GPS code-measurement and WLS analysis model.""" diff --git a/soc/software/gps_measurement_python/analyze_doppler_eskf.py b/soc/software/gps_measurement_python/analyze_doppler_eskf.py new file mode 100644 index 0000000..126189f --- /dev/null +++ b/soc/software/gps_measurement_python/analyze_doppler_eskf.py @@ -0,0 +1,428 @@ +"""Monte Carlo comparison of Doppler prefilter choices before velocity ESKF.""" + +from __future__ import annotations + +import argparse +import csv +import math +from concurrent.futures import ProcessPoolExecutor +from dataclasses import dataclass +from pathlib import Path + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np + +from .doppler_eskf_model import ( + DopplerEskfTrace, + DopplerPath, + PATH_ORDER, + VELOCITY_NIS_GATE, + flight_scenarios, + run_doppler_eskf_scenario, +) + + +PATH_LABEL = { + DopplerPath.FIXED_IIR: "Önceki sabit IIR", + DopplerPath.RAW: "Ham Doppler + ESKF", + DopplerPath.ADAPTIVE_IIR: "Adaptif IIR + ESKF", + DopplerPath.RAW_ROBUST: "Ham + robust WLS + ESKF", +} + + +@dataclass(frozen=True) +class TrialMetrics: + scenario: str + path: DopplerPath + fused_rms_mps: float + fused_p95_mps: float + quiet_rms_mps: float + dynamic_rms_mps: float + wls_rms_mps: float + wls_p95_mps: float + clock_drift_rms_mps: float + velocity_accept_percent: float + nis_reject_percent: float + fused_95_coverage_percent: float + excluded_epoch_percent: float + mean_covariance_scale: float + mean_alpha: float + + +@dataclass(frozen=True) +class Summary: + scenario: str + path: DopplerPath + seeds: int + fused_rms_mps: float + fused_rms_seed_std_mps: float + fused_p95_mps: float + quiet_rms_mps: float + dynamic_rms_mps: float + wls_rms_mps: float + wls_p95_mps: float + clock_drift_rms_mps: float + velocity_accept_percent: float + nis_reject_percent: float + fused_95_coverage_percent: float + excluded_epoch_percent: float + mean_covariance_scale: float + mean_alpha: float + + +def _rms(values: np.ndarray) -> float: + return float(np.sqrt(np.mean(np.square(values)))) if len(values) else math.nan + + +def _truth_at_wls(trace: DopplerEskfTrace) -> np.ndarray: + return np.column_stack( + [ + np.interp( + trace.wls_time_s, + trace.time_s, + trace.truth_velocity_mps[:, axis], + ) + for axis in range(3) + ] + ) + + +def trial_metrics(scenario_index: int, trace: DopplerEskfTrace) -> TrialMetrics: + scenario = flight_scenarios()[scenario_index] + burn_in = trace.time_s >= 5.0 + fused_vector_error = trace.fused_velocity_mps - trace.truth_velocity_mps + fused_error = np.linalg.norm(fused_vector_error, axis=1) + dynamic = burn_in & (np.linalg.norm(trace.acceleration_mps2, axis=1) > 0.25) + quiet = burn_in & ~dynamic + + fused_nees = [] + for sample in np.flatnonzero(burn_in): + covariance = trace.fused_velocity_covariance_mps2[sample] + error = fused_vector_error[sample] + fused_nees.append(float(error @ np.linalg.inv(covariance) @ error)) + fused_nees_array = np.asarray(fused_nees) + + wls_error = np.linalg.norm(trace.wls_velocity_mps - _truth_at_wls(trace), axis=1) + true_clock_drift = np.asarray( + [scenario.receiver_clock_drift_mps(float(time)) for time in trace.wls_time_s] + ) + clock_error = trace.wls_clock_drift_mps - true_clock_drift + valid_nis = trace.wls_nis[np.isfinite(trace.wls_nis)] + + return TrialMetrics( + scenario=trace.scenario, + path=trace.path, + fused_rms_mps=_rms(fused_error[burn_in]), + fused_p95_mps=float(np.percentile(fused_error[burn_in], 95.0)), + quiet_rms_mps=_rms(fused_error[quiet]), + dynamic_rms_mps=_rms(fused_error[dynamic]), + wls_rms_mps=_rms(wls_error), + wls_p95_mps=float(np.percentile(wls_error, 95.0)), + clock_drift_rms_mps=_rms(clock_error), + velocity_accept_percent=100.0 * float(np.mean(trace.wls_accepted)), + nis_reject_percent=100.0 * float(np.mean(valid_nis > VELOCITY_NIS_GATE)), + # 7.8147 is the 95-percent chi-square threshold for three states. + fused_95_coverage_percent=100.0 * float(np.mean(fused_nees_array <= 7.8147279)), + excluded_epoch_percent=100.0 + * float(np.mean(trace.excluded_satellites >= 0)), + mean_covariance_scale=float(np.mean(trace.wls_covariance_scale)), + mean_alpha=float(np.mean(trace.adaptive_alpha)), + ) + + +def _worker(task: tuple[int, int]) -> tuple[int, int, list[TrialMetrics]]: + scenario_index, seed = task + scenario = flight_scenarios()[scenario_index] + traces = run_doppler_eskf_scenario(scenario, seed) + return ( + scenario_index, + seed, + [trial_metrics(scenario_index, traces[path]) for path in PATH_ORDER], + ) + + +def summarize(items: list[TrialMetrics]) -> Summary: + if not items: + raise ValueError("cannot summarize an empty trial list") + + def average(field_name: str) -> float: + values = np.asarray([getattr(item, field_name) for item in items], dtype=float) + finite = values[np.isfinite(values)] + return float(np.mean(finite)) if len(finite) else math.nan + + fused_rms = np.asarray([item.fused_rms_mps for item in items]) + return Summary( + scenario=items[0].scenario, + path=items[0].path, + seeds=len(items), + fused_rms_mps=average("fused_rms_mps"), + fused_rms_seed_std_mps=float(np.std(fused_rms)), + fused_p95_mps=average("fused_p95_mps"), + quiet_rms_mps=average("quiet_rms_mps"), + dynamic_rms_mps=average("dynamic_rms_mps"), + wls_rms_mps=average("wls_rms_mps"), + wls_p95_mps=average("wls_p95_mps"), + clock_drift_rms_mps=average("clock_drift_rms_mps"), + velocity_accept_percent=average("velocity_accept_percent"), + nis_reject_percent=average("nis_reject_percent"), + fused_95_coverage_percent=average("fused_95_coverage_percent"), + excluded_epoch_percent=average("excluded_epoch_percent"), + mean_covariance_scale=average("mean_covariance_scale"), + mean_alpha=average("mean_alpha"), + ) + + +def write_csv(path: Path, summaries: list[Summary]) -> None: + with path.open("w", newline="", encoding="utf-8") as output: + writer = csv.DictWriter(output, fieldnames=list(Summary.__dataclass_fields__)) + writer.writeheader() + for item in summaries: + row = dict(item.__dict__) + row["path"] = item.path.value + writer.writerow(row) + + +def _summary_plot(path: Path, summaries: list[Summary]) -> None: + scenarios = flight_scenarios() + x = np.arange(len(scenarios)) + width = 0.19 + figure, axis = plt.subplots(figsize=(15, 6.3)) + for path_index, doppler_path in enumerate(PATH_ORDER): + values = [ + next( + item.fused_rms_mps + for item in summaries + if item.scenario == scenario.name and item.path == doppler_path + ) + for scenario in scenarios + ] + axis.bar( + x + (path_index - 1.5) * width, + values, + width, + label=PATH_LABEL[doppler_path], + ) + axis.set_ylabel("ESKF 3D velocity RMS (m/s)") + axis.set_yscale("log") + axis.set_xticks(x) + axis.set_xticklabels([item.name for item in scenarios], rotation=25, ha="right") + axis.grid(axis="y", alpha=0.25) + axis.legend(ncol=2) + figure.tight_layout() + figure.savefig(path, dpi=150) + plt.close(figure) + + +def _maneuver_trace_plot(path: Path, trace_set: dict[DopplerPath, DopplerEskfTrace]) -> None: + reference = trace_set[DopplerPath.FIXED_IIR] + figure, axes = plt.subplots(3, 1, figsize=(13, 8.5), sharex=True) + for doppler_path in PATH_ORDER: + trace = trace_set[doppler_path] + error = np.linalg.norm(trace.fused_velocity_mps - trace.truth_velocity_mps, axis=1) + axes[0].plot(trace.time_s, error, label=PATH_LABEL[doppler_path], linewidth=1.0) + axes[1].plot( + reference.time_s, + np.linalg.norm(reference.acceleration_mps2, axis=1), + color="black", + linewidth=1.2, + label="True |acceleration|", + ) + adaptive = trace_set[DopplerPath.ADAPTIVE_IIR] + axes[2].step( + adaptive.wls_time_s, + adaptive.adaptive_alpha, + where="post", + color="tab:green", + ) + axes[0].set_ylabel("3D velocity error (m/s)") + axes[1].set_ylabel("Acceleration (m/s²)") + axes[2].set_ylabel("Adaptive alpha") + axes[2].set_xlabel("Time (s)") + axes[0].set_ylim(bottom=0.0) + axes[0].legend(ncol=2) + for axis in axes: + axis.grid(alpha=0.25) + figure.tight_layout() + figure.savefig(path, dpi=150) + plt.close(figure) + + +def _outlier_plot(path: Path, trace_set: dict[DopplerPath, DopplerEskfTrace]) -> None: + figure, axes = plt.subplots(2, 1, figsize=(13, 7), sharex=True) + for doppler_path in PATH_ORDER: + trace = trace_set[doppler_path] + error = np.linalg.norm(trace.wls_velocity_mps - _truth_at_wls(trace), axis=1) + axes[0].plot(trace.wls_time_s, error, label=PATH_LABEL[doppler_path], linewidth=1.0) + axes[1].plot(trace.wls_time_s, trace.wls_nis, label=PATH_LABEL[doppler_path], linewidth=0.9) + axes[1].axhline(VELOCITY_NIS_GATE, color="black", linestyle="--", linewidth=1.0) + for start, stop in ((30.0, 31.0), (56.0, 57.0), (72.0, 72.6)): + for axis in axes: + axis.axvspan(start, stop, color="tab:red", alpha=0.10) + axes[0].set_ylabel("WLS velocity error (m/s)") + axes[1].set_ylabel("Velocity NIS") + axes[1].set_xlabel("Time (s)") + axes[0].legend(ncol=2) + for axis in axes: + axis.grid(alpha=0.25) + figure.tight_layout() + figure.savefig(path, dpi=150) + plt.close(figure) + + +def _filtering_plot(path: Path, trace_set: dict[DopplerPath, DopplerEskfTrace]) -> None: + figure, axes = plt.subplots(2, 1, figsize=(13, 7), sharex=True) + for doppler_path in (DopplerPath.FIXED_IIR, DopplerPath.RAW, DopplerPath.ADAPTIVE_IIR): + trace = trace_set[doppler_path] + wls_error = np.linalg.norm(trace.wls_velocity_mps - _truth_at_wls(trace), axis=1) + fused_error = np.linalg.norm(trace.fused_velocity_mps - trace.truth_velocity_mps, axis=1) + axes[0].plot(trace.wls_time_s, wls_error, label=PATH_LABEL[doppler_path], linewidth=0.9) + axes[1].plot(trace.time_s, fused_error, label=PATH_LABEL[doppler_path], linewidth=0.9) + axes[0].set_ylabel("WLS 3D error (m/s)") + axes[1].set_ylabel("ESKF 3D error (m/s)") + axes[1].set_xlabel("Time (s)") + axes[0].legend(ncol=2) + for axis in axes: + axis.grid(alpha=0.25) + figure.tight_layout() + figure.savefig(path, dpi=150) + plt.close(figure) + + +def _lookup( + summaries: list[Summary], scenario: str, doppler_path: DopplerPath +) -> Summary: + return next( + item + for item in summaries + if item.scenario == scenario and item.path == doppler_path + ) + + +def write_report(path: Path, summaries: list[Summary], seed_count: int) -> None: + lines = [ + "# Doppler WLS -> ESKF model sonuçları", + "", + f"Monte Carlo seed sayısı: `{seed_count}`; ilk 5 saniye RMS metriklerinden çıkarıldı.", + "50 Hz tracking, önceki `alpha=0.05` Doppler IIR, 5 Hz dört-state velocity WLS, fusion adapter'ın `2/2/3 m/s` ENU sigma tabanları, TDK profiliyle 100 Hz IMU propagation ve 3-DOF `%99.9` NIS kapısı modellenmiştir.", + "", + "## Sonuç tablosu", + "", + "| Senaryo | Yol | ESKF RMS (m/s) | Dynamic RMS | WLS RMS | Accept (%) | 95% cov. (%) | Exclude (%) |", + "|---|---|---:|---:|---:|---:|---:|---:|", + ] + for scenario in flight_scenarios(): + for doppler_path in PATH_ORDER: + item = _lookup(summaries, scenario.name, doppler_path) + dynamic = "-" if not np.isfinite(item.dynamic_rms_mps) else f"{item.dynamic_rms_mps:.3f}" + lines.append( + f"| {scenario.name} | {PATH_LABEL[doppler_path]} | " + f"{item.fused_rms_mps:.3f} | {dynamic} | {item.wls_rms_mps:.3f} | " + f"{item.velocity_accept_percent:.1f} | {item.fused_95_coverage_percent:.1f} | " + f"{item.excluded_epoch_percent:.1f} |" + ) + + steady_fixed = _lookup(summaries, "steady_cruise", DopplerPath.FIXED_IIR) + steady_raw = _lookup(summaries, "steady_cruise", DopplerPath.RAW) + air_fixed = _lookup(summaries, "airliner_maneuver", DopplerPath.FIXED_IIR) + air_raw = _lookup(summaries, "airliner_maneuver", DopplerPath.RAW) + weak_fixed = _lookup(summaries, "low_cn0_airliner", DopplerPath.FIXED_IIR) + weak_raw = _lookup(summaries, "low_cn0_airliner", DopplerPath.RAW) + weak_adaptive = _lookup(summaries, "low_cn0_airliner", DopplerPath.ADAPTIVE_IIR) + outlier_raw = _lookup(summaries, "single_satellite_outliers", DopplerPath.RAW) + outlier_robust = _lookup(summaries, "single_satellite_outliers", DopplerPath.RAW_ROBUST) + clock_raw = _lookup(summaries, "clock_drift_steps", DopplerPath.RAW) + + lines.extend( + [ + "", + "## Bulgular", + "", + f"- Sabit hızda pre-IIR ESKF RMS'i `{steady_fixed.fused_rms_mps:.3f}` m/s, ham yol `{steady_raw.fused_rms_mps:.3f}` m/s. Yani pre-IIR sakin durumda daha düzgün çıktı veriyor; ESKF ham gürültüyü zaten `{steady_raw.wls_rms_mps:.3f}` m/s WLS RMS'ten bu seviyeye indiriyor.", + f"- Airliner manevrasında sabit IIR `{air_fixed.fused_rms_mps:.3f}` m/s ve `%{air_fixed.nis_reject_percent:.1f}` NIS ret oranı verirken ham yol `{air_raw.fused_rms_mps:.3f}` m/s ve `%{air_raw.nis_reject_percent:.1f}` ret verdi. Konservatif adapter tabanları rejection'ı sınırlasa bile covariance'ın temsil etmediği IIR gecikmesi fused velocity'de dinamik hata bırakır.", + f"- Düşük C/N0 manevrasında sabit/ham/adaptif RMS sırasıyla `{weak_fixed.fused_rms_mps:.3f}`, `{weak_raw.fused_rms_mps:.3f}`, `{weak_adaptive.fused_rms_mps:.3f}` m/s oldu. Adaptif yol bu sentetik koşulda gürültü/gecikme uzlaşmasını sağladı; gerçek eşikler kart verisi ister.", + f"- Tek uydu burst'ünde raw WLS RMS `{outlier_raw.wls_rms_mps:.3f}` m/s, robust leave-one-out ile `{outlier_robust.wls_rms_mps:.3f}` m/s. ESKF son RMS farkı daha küçük; çünkü mevcut residual covariance inflation aggregate hatayı zaten zayıflatıyor. Robust kapı yine de hatalı kanalı bütün epoch'u reddetmeden ayırır.", + f"- Common clock-drift step raw velocity RMS'ini bozmadı; clock-drift state RMS'i `{clock_raw.clock_drift_rms_mps:.3f}` m/s. Dördüncü WLS bilinmeyeni ortak frekans hatasını üç hız bileşeninden ayırıyor.", + "", + "## 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.", + "- 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.", + "", + "## Model sınırları", + "", + "- Sonuçlar sentetik hata büyüklüklerine koşulludur; kartta yüzde kazanç iddiası değildir.", + "- Uydu geometrisi sabit sekiz-kanal local LOS modelidir. Satellite velocity/clock düzeltmelerinin doğru yapıldığı varsayılmıştır.", + "- ESKF, bu soruya etki eden `[velocity, accel-bias]` alt sistemine indirgenmiştir; attitude, lever-arm ve tam position cross-covariance modellenmemiştir.", + "- Tam firmware'deki position update'in velocity/accel-bias cross-covariance üzerinden sağlayabileceği kısmi recovery bu azaltılmış modelde yoktur. Bu nedenle uzun NIS-ret zincirinin mutlak büyüklüğü değil, pre-IIR'in gecikme ve ret mekanizması karar girdisidir.", + "- Carrier loop gürültüsü C/N0 sigma yasasıyla üretilmiştir. Gerçek correlation, oscillator phase noise ve cycle slip dağılımları kart loguyla kalibre edilmelidir.", + "- Robust WLS'nin `3.5 sigma`, en az altı uydu ve bütün tek-kanal leave-one-out seçimi C implementasyonuyla aynıdır. RTL değişmez; raw/filtered ayrımı firmware'dedir.", + "", + ] + ) + path.write_text("\n".join(lines), encoding="utf-8") + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--seeds", type=int, default=20) + parser.add_argument("--jobs", type=int, default=4) + parser.add_argument( + "--output", + type=Path, + default=Path(__file__).resolve().parent / "output", + ) + arguments = parser.parse_args() + arguments.output.mkdir(parents=True, exist_ok=True) + + scenarios = flight_scenarios() + tasks = [ + (scenario_index, seed) + for scenario_index in range(len(scenarios)) + for seed in range(1, arguments.seeds + 1) + ] + grouped: dict[tuple[int, DopplerPath], list[TrialMetrics]] = { + (scenario_index, doppler_path): [] + for scenario_index in range(len(scenarios)) + for doppler_path in PATH_ORDER + } + with ProcessPoolExecutor(max_workers=max(1, arguments.jobs)) as executor: + for scenario_index, seed, metrics in executor.map(_worker, tasks): + for item in metrics: + grouped[(scenario_index, item.path)].append(item) + print(f"completed {scenarios[scenario_index].name} seed={seed}", flush=True) + + summaries = [ + summarize(grouped[(scenario_index, doppler_path)]) + for scenario_index in range(len(scenarios)) + for doppler_path in PATH_ORDER + ] + write_csv(arguments.output / "doppler_eskf_summary.csv", summaries) + write_report( + arguments.output / "DOPPLER_ESKF_ANALYSIS.md", + summaries, + arguments.seeds, + ) + _summary_plot(arguments.output / "05_doppler_eskf_velocity_rms.png", summaries) + + airliner = run_doppler_eskf_scenario( + next(item for item in scenarios if item.name == "airliner_maneuver"), 1 + ) + _maneuver_trace_plot(arguments.output / "06_airliner_maneuver_trace.png", airliner) + outlier = run_doppler_eskf_scenario( + next(item for item in scenarios if item.name == "single_satellite_outliers"), 1 + ) + _outlier_plot(arguments.output / "07_outlier_wls_nis.png", outlier) + weak = run_doppler_eskf_scenario( + next(item for item in scenarios if item.name == "low_cn0_airliner"), 1 + ) + _filtering_plot(arguments.output / "08_low_cn0_wls_vs_eskf.png", weak) + print(f"wrote Doppler/ESKF analysis to {arguments.output}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/soc/software/gps_measurement_python/analyze_measurement_scenarios.py b/soc/software/gps_measurement_python/analyze_measurement_scenarios.py new file mode 100644 index 0000000..ff3b617 --- /dev/null +++ b/soc/software/gps_measurement_python/analyze_measurement_scenarios.py @@ -0,0 +1,425 @@ +"""Run the complete DLL-quality/WLS and code-smoothing comparison.""" + +from __future__ import annotations + +import argparse +import csv +import math +from concurrent.futures import ProcessPoolExecutor +from dataclasses import dataclass +from pathlib import Path + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np + +from .code_smoothing_model import run_smoothing_trial +from .firmware_wls_model import SigmaMethod +from .measurement_scenarios import EpochSample, run_scenario, scenario_matrix + + +METHOD_ORDER = ( + SigmaMethod.CURRENT, + SigmaMethod.NAIVE_ADD_DLL, + SigmaMethod.DLL_EXCESS, + SigmaMethod.DLL_EXCESS_GUARD, + SigmaMethod.ORACLE, +) +METHOD_LABEL = { + SigmaMethod.CURRENT: "Mevcut C/N0", + SigmaMethod.NAIVE_ADD_DLL: "Naif DLL ekle", + SigmaMethod.DLL_EXCESS: "DLL excess", + SigmaMethod.DLL_EXCESS_GUARD: "DLL excess+mean", + SigmaMethod.ORACLE: "Oracle variance", +} + + +@dataclass(frozen=True) +class Summary: + scenario: str + method: SigmaMethod + epochs: int + horizontal_rms_m: float + position_3d_rms_m: float + position_3d_p95_m: float + valid_percent: float + valid_3d_rms_m: float + horizontal_2sigma_coverage_percent: float + false_valid_over_25m_percent: float + fde_percent: float + mean_measurement_sigma_m: float + + +def _worker(task: tuple[int, int]) -> tuple[int, int, list[EpochSample]]: + scenario_index, seed = task + scenario = scenario_matrix()[scenario_index] + return scenario_index, seed, run_scenario(scenario, seed, METHOD_ORDER) + + +def _rms(values: np.ndarray) -> float: + return float(np.sqrt(np.mean(np.square(values)))) if len(values) else math.nan + + +def summarize(scenario_name: str, method: SigmaMethod, samples: list[EpochSample]) -> Summary: + selected = [ + item + for item in samples + if item.method == method and item.time_s >= 5.0 + ] + horizontal = np.asarray([item.horizontal_error_m for item in selected]) + error_3d = np.asarray([item.position_error_3d_m for item in selected]) + valid = np.asarray([item.valid for item in selected], dtype=bool) + horizontal_sigma = np.asarray([item.horizontal_sigma_m for item in selected]) + fde = np.asarray([item.fde_applied for item in selected], dtype=bool) + mean_sigma = np.asarray([item.mean_measurement_sigma_m for item in selected]) + false_valid = valid & (error_3d > 25.0) + return Summary( + scenario=scenario_name, + method=method, + epochs=len(selected), + horizontal_rms_m=_rms(horizontal), + position_3d_rms_m=_rms(error_3d), + position_3d_p95_m=float(np.percentile(error_3d, 95.0)), + valid_percent=100.0 * float(np.mean(valid)), + valid_3d_rms_m=_rms(error_3d[valid]), + horizontal_2sigma_coverage_percent=100.0 + * float(np.mean(horizontal <= 2.0 * horizontal_sigma)), + false_valid_over_25m_percent=100.0 * float(np.mean(false_valid)), + fde_percent=100.0 * float(np.mean(fde)), + mean_measurement_sigma_m=float(np.mean(mean_sigma)), + ) + + +def write_csv(path: Path, summaries: list[Summary]) -> None: + with path.open("w", newline="", encoding="utf-8") as output: + writer = csv.DictWriter(output, fieldnames=list(Summary.__dataclass_fields__)) + writer.writeheader() + for item in summaries: + row = dict(item.__dict__) + row["method"] = item.method.value + writer.writerow(row) + + +def _comparison_plot( + path: Path, + summaries: list[Summary], + field: str, + ylabel: str, + *, + logarithmic: bool = False, +) -> None: + scenarios = scenario_matrix() + x = np.arange(len(scenarios)) + width = 0.16 + figure, axis = plt.subplots(figsize=(15, 6.5)) + for method_index, method in enumerate(METHOD_ORDER): + values = [ + getattr( + next( + item + for item in summaries + if item.scenario == scenario.name and item.method == method + ), + field, + ) + for scenario in scenarios + ] + axis.bar( + x + (method_index - 2) * width, + values, + width, + label=METHOD_LABEL[method], + ) + axis.set_ylabel(ylabel) + if logarithmic: + axis.set_yscale("log") + axis.set_xticks(x) + axis.set_xticklabels([item.name for item in scenarios], rotation=28, ha="right") + axis.grid(axis="y", alpha=0.25) + axis.legend(ncol=3) + figure.tight_layout() + figure.savefig(path, dpi=150) + plt.close(figure) + + +def _transition_plot(path: Path, samples: list[EpochSample]) -> None: + figure, axes = plt.subplots(2, 1, figsize=(12, 7), sharex=True) + for method in ( + SigmaMethod.CURRENT, + SigmaMethod.DLL_EXCESS, + SigmaMethod.DLL_EXCESS_GUARD, + ): + selected = [item for item in samples if item.method == method] + axes[0].plot( + [item.time_s for item in selected], + [item.position_error_3d_m for item in selected], + label=METHOD_LABEL[method], + linewidth=1.2, + ) + reference = [item for item in samples if item.method == SigmaMethod.CURRENT] + axes[1].plot( + [item.time_s for item in reference], + [item.maximum_dll_ratio for item in reference], + color="tab:red", + ) + axes[0].axvspan(22.0, 45.0, color="tab:orange", alpha=0.15) + axes[1].axvspan(22.0, 45.0, color="tab:orange", alpha=0.15) + axes[0].set_ylabel("3D konum hatası (m)") + axes[1].set_ylabel("Maks. DLL variance ratio") + axes[1].set_xlabel("Zaman (s)") + axes[0].legend() + for axis in axes: + axis.grid(alpha=0.25) + figure.tight_layout() + figure.savefig(path, dpi=150) + plt.close(figure) + + +def smoothing_summary(seed_count: int) -> dict[str, dict[str, float]]: + result: dict[str, dict[str, float]] = {} + for label, cfo_mps in ( + ("cfo_compensated", 0.0), + ("cfo_residual_0p5_mps", 0.5), + ("cfo_residual_3_mps", 3.0), + ): + trials = [ + run_smoothing_trial(seed, doppler_cfo_mps=cfo_mps) + for seed in range(1, seed_count + 1) + ] + result[label] = { + field: float(np.mean([getattr(item, field) for item in trials])) + for field in trials[0].__dataclass_fields__ + } + return result + + +def _smoothing_plot(path: Path, smoothing: dict[str, dict[str, float]]) -> None: + labels = list(smoothing) + x = np.arange(len(labels)) + width = 0.20 + figure, axis = plt.subplots(figsize=(10, 5.5)) + for offset, (field, title) in enumerate( + ( + ("raw_rmse_m", "Ham code"), + ("scalar_iir_rmse_m", "Kör IIR"), + ("doppler_smoothed_rmse_m", "Raw Doppler-predicted"), + ( + "firmware_iir_doppler_smoothed_rmse_m", + "FW-IIR Doppler-predicted", + ), + ) + ): + axis.bar( + x + (offset - 1.5) * width, + [smoothing[label][field] for label in labels], + width, + label=title, + ) + axis.set_xticks(x) + axis.set_xticklabels(labels) + axis.set_ylabel("Range RMSE (m)") + axis.set_yscale("log") + axis.grid(axis="y", alpha=0.25) + axis.legend() + figure.tight_layout() + figure.savefig(path, dpi=150) + plt.close(figure) + + +def write_report( + path: Path, + summaries: list[Summary], + focused_rms: dict[str, dict[SigmaMethod, float]], + smoothing: dict[str, dict[str, float]], + seed_count: int, +) -> None: + current = { + item.scenario: item + for item in summaries + if item.method == SigmaMethod.CURRENT + } + lines = [ + "# GPS code measurement model sonuçları", + "", + f"Monte Carlo seed sayısı: `{seed_count}`. İlk 5 saniye metriklere alınmadı.", + "", + "## Yöntem karşılaştırması", + "", + "| Senaryo | Yöntem | 3D RMS (m) | Mevcuda göre | P95 (m) | Valid (%) | >25 m false-valid (%) |", + "|---|---|---:|---:|---:|---:|---:|", + ] + for scenario in scenario_matrix(): + baseline = current[scenario.name].position_3d_rms_m + for method in METHOD_ORDER: + item = next( + value + for value in summaries + if value.scenario == scenario.name and value.method == method + ) + improvement = 100.0 * (baseline - item.position_3d_rms_m) / baseline + lines.append( + f"| {scenario.name} | {METHOD_LABEL[method]} | " + f"{item.position_3d_rms_m:.2f} | {improvement:+.1f}% | " + f"{item.position_3d_p95_m:.2f} | {item.valid_percent:.1f} | " + f"{item.false_valid_over_25m_percent:.2f} |" + ) + + lines.extend( + [ + "", + "## Modelden çıkan karar özeti", + "", + "- C/N0 ile uyumlu nominal DLL jitter'da `dll_excess`, mevcut çözümü değiştirmemelidir.", + "- Bütün measurement sigmaları aynı sabitle çarpılırsa WLS konum noktası değişmez; yalnız covariance/integrity değişir. Fakat ortak DLL jitter çarpanı, C/N0/elevation bileşimi kanaldan kanala farklı olduğu için toplam sigmaları eşit oranda büyütmez.", + "- Kanal-bağımlı ve C/N0'da görünmeyen excess jitter varsa dinamik sigma o kanalı down-weight ederek konumu iyileştirebilir.", + "- Stable multipath bias düşük DLL variance bırakabildiği için variance-only yöntem çözüm değildir.", + "- 50 Hz'de DLL ratio alpha=0.10 yaklaşık 0.19 s zaman sabiti ve 0.57 s yüzde-95 geçiş verir.", + "- Aşağıdaki yüzdeler seçilen sentetik hata büyüklüklerine koşulludur; kart için kazanç tahmini değildir.", + "", + "## Bozulma penceresi odaklı 3D RMS", + "", + "| Senaryo/pencere | Mevcut (m) | DLL excess (m) | DLL excess+mean (m) | Oracle variance (m) |", + "|---|---:|---:|---:|---:|", + ] + ) + for label, values in focused_rms.items(): + lines.append( + f"| {label} | {values[SigmaMethod.CURRENT]:.2f} | " + f"{values[SigmaMethod.DLL_EXCESS]:.2f} | " + f"{values[SigmaMethod.DLL_EXCESS_GUARD]:.2f} | " + f"{values[SigmaMethod.ORACLE]:.2f} |" + ) + lines.extend( + [ + "", + "## Code smoothing ve carrier CFO", + "", + "| Carrier-rate koşulu | Ham RMSE (m) | Kör IIR RMSE (m) | Raw Doppler RMSE (m) | FW-IIR Doppler RMSE (m) | Kör IIR peak (m) | Raw Doppler peak (m) | FW-IIR Doppler peak (m) |", + "|---|---:|---:|---:|---:|---:|---:|---:|", + ] + ) + for label, values in smoothing.items(): + lines.append( + f"| {label} | {values['raw_rmse_m']:.2f} | " + f"{values['scalar_iir_rmse_m']:.2f} | " + f"{values['doppler_smoothed_rmse_m']:.2f} | " + f"{values['firmware_iir_doppler_smoothed_rmse_m']:.2f} | " + f"{values['scalar_iir_peak_dynamic_error_m']:.2f} | " + f"{values['doppler_peak_dynamic_error_m']:.2f} | " + f"{values['firmware_iir_doppler_peak_dynamic_error_m']:.2f} |" + ) + + lines.extend( + [ + "", + "## Yorum sınırları", + "", + "- `current_cn0`, firmware'deki URA/elevation/C/N0 sigma modelidir.", + "- `naive_add_dll`, aynı thermal jitter'ı iki kere saymanın negatif kontrolüdür.", + "- `dll_excess`, yalnız ölçülen DLL variance'ın C/N0 beklentisini aşan kısmını code variance'a ekler.", + "- `dll_excess_guard`, variance'a ek olarak sürekli discriminator mean stress'ini kullanır.", + "- `oracle_variance`, sentetik gerçek jitter çarpanını bilir; yalnız variance kaynakları için erişilebilir üst sınırdır, bias'ı bilmez.", + "- Stable multipath DLL'nin yeni denge noktasında düşük residual variance bırakabilir. Bu nedenle variance-only yönteminin onu çözmesi beklenmez.", + "- Discriminator gürültü ölçeği kart correlator kayıtları gelene kadar kalibrasyon varsayımıdır; oran tabanlı yöntem mutlak ölçeğe göre daha az hassastır.", + "- Sonuçlar bitstream veya firmware değişikliği değildir; karar öncesi executable tasarım keşfidir.", + "", + ] + ) + path.write_text("\n".join(lines), encoding="utf-8") + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--seeds", type=int, default=12) + parser.add_argument("--jobs", type=int, default=4) + parser.add_argument( + "--output", + type=Path, + default=Path(__file__).resolve().parent / "output", + ) + arguments = parser.parse_args() + arguments.output.mkdir(parents=True, exist_ok=True) + + scenarios = scenario_matrix() + tasks = [ + (scenario_index, seed) + for scenario_index in range(len(scenarios)) + for seed in range(1, arguments.seeds + 1) + ] + grouped: dict[int, list[EpochSample]] = { + index: [] for index in range(len(scenarios)) + } + transition_samples: list[EpochSample] = [] + with ProcessPoolExecutor(max_workers=max(1, arguments.jobs)) as executor: + for scenario_index, seed, samples in executor.map(_worker, tasks): + grouped[scenario_index].extend(samples) + if scenarios[scenario_index].name == "noise_burst_recovery" and seed == 1: + transition_samples = samples + print( + f"completed {scenarios[scenario_index].name} seed={seed}", + flush=True, + ) + + summaries = [ + summarize(scenario.name, method, grouped[index]) + for index, scenario in enumerate(scenarios) + for method in METHOD_ORDER + ] + focused_windows = { + "noise_burst_recovery [22,45) s": ("noise_burst_recovery", 22.0, 45.0), + "dll_transient_stress [25,34) s": ("dll_transient_stress", 25.0, 34.0), + "cn0_step_recovery [5,30) s": ("cn0_step_recovery", 5.0, 30.0), + } + focused_rms: dict[str, dict[SigmaMethod, float]] = {} + for label, (scenario_name, start_s, stop_s) in focused_windows.items(): + scenario_index = next( + index + for index, scenario in enumerate(scenarios) + if scenario.name == scenario_name + ) + focused_rms[label] = {} + for method in METHOD_ORDER: + values = np.asarray( + [ + item.position_error_3d_m + for item in grouped[scenario_index] + if item.method == method + and start_s <= item.time_s < stop_s + ] + ) + focused_rms[label][method] = _rms(values) + smoothing = smoothing_summary(max(arguments.seeds, 20)) + + write_csv(arguments.output / "scenario_summary.csv", summaries) + write_report( + arguments.output / "ANALYSIS.md", + summaries, + focused_rms, + smoothing, + arguments.seeds, + ) + _comparison_plot( + arguments.output / "01_position_3d_rms.png", + summaries, + "position_3d_rms_m", + "3D RMS (m)", + logarithmic=True, + ) + _comparison_plot( + arguments.output / "02_solution_availability.png", + summaries, + "valid_percent", + "Integrity-valid çözüm (%)", + ) + _transition_plot( + arguments.output / "03_noise_burst_transition.png", + transition_samples, + ) + _smoothing_plot(arguments.output / "04_code_smoothing_cfo.png", smoothing) + print(f"wrote analysis to {arguments.output}", flush=True) + + +if __name__ == "__main__": + main() diff --git a/soc/software/gps_measurement_python/code_smoothing_model.py b/soc/software/gps_measurement_python/code_smoothing_model.py new file mode 100644 index 0000000..83b56d4 --- /dev/null +++ b/soc/software/gps_measurement_python/code_smoothing_model.py @@ -0,0 +1,132 @@ +"""One-dimensional code smoothing study with carrier/Doppler aiding.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass(frozen=True) +class SmoothingResult: + raw_rmse_m: float + scalar_iir_rmse_m: float + doppler_smoothed_rmse_m: float + firmware_iir_doppler_smoothed_rmse_m: float + scalar_iir_peak_dynamic_error_m: float + doppler_peak_dynamic_error_m: float + firmware_iir_doppler_peak_dynamic_error_m: float + + +def run_smoothing_trial( + seed: int, + *, + duration_s: float = 80.0, + tracking_dt_s: float = 0.020, + wls_dt_s: float = 0.200, + code_sigma_m: float = 4.0, + doppler_sigma_mps: float = 0.25, + doppler_cfo_mps: float = 0.0, + alpha: float = 0.10, + firmware_doppler_alpha: float = 0.05, +) -> SmoothingResult: + """Compare raw code, blind IIR and Doppler-propagated code smoothing. + + The truth includes a range-rate step so a visually smooth but lagging + scalar IIR is penalized. ``doppler_cfo_mps`` represents residual receiver + carrier frequency offset after the nominal IF subtraction. + """ + + if abs(round(wls_dt_s / tracking_dt_s) - wls_dt_s / tracking_dt_s) > 1.0e-12: + raise ValueError("WLS period must be an integer number of tracking periods") + rng = np.random.default_rng(seed) + tracking_time_s = np.arange(0.0, duration_s, tracking_dt_s) + true_rate_tracking = np.where( + tracking_time_s < 20.0, + -120.0, + np.where(tracking_time_s < 45.0, 230.0, -40.0), + ) + true_range_tracking = np.empty_like(tracking_time_s) + true_range_tracking[0] = 21_000_000.0 + true_range_tracking[1:] = ( + true_range_tracking[0] + + np.cumsum(true_rate_tracking[:-1] * tracking_dt_s) + ) + measured_rate_tracking = ( + true_rate_tracking + + doppler_cfo_mps + + rng.normal(0.0, doppler_sigma_mps, len(tracking_time_s)) + ) + filtered_rate_tracking = np.empty_like(measured_rate_tracking) + filtered_rate_tracking[0] = measured_rate_tracking[0] + for index in range(1, len(filtered_rate_tracking)): + filtered_rate_tracking[index] = ( + firmware_doppler_alpha * measured_rate_tracking[index] + + (1.0 - firmware_doppler_alpha) * filtered_rate_tracking[index - 1] + ) + + tracking_per_wls = int(round(wls_dt_s / tracking_dt_s)) + wls_indices = np.arange(0, len(tracking_time_s), tracking_per_wls) + time_s = tracking_time_s[wls_indices] + true_range = true_range_tracking[wls_indices] + raw_code = true_range + rng.normal(0.0, code_sigma_m, len(time_s)) + + scalar = np.empty_like(raw_code) + doppler_smoothed = np.empty_like(raw_code) + firmware_iir_doppler_smoothed = np.empty_like(raw_code) + scalar[0] = raw_code[0] + doppler_smoothed[0] = raw_code[0] + firmware_iir_doppler_smoothed[0] = raw_code[0] + for index in range(1, len(time_s)): + scalar[index] = alpha * raw_code[index] + (1.0 - alpha) * scalar[index - 1] + start = wls_indices[index - 1] + stop = wls_indices[index] + raw_range_delta = ( + float(np.sum(measured_rate_tracking[start:stop])) + * tracking_dt_s + ) + filtered_range_delta = ( + float(np.sum(filtered_rate_tracking[start:stop])) + * tracking_dt_s + ) + prediction = doppler_smoothed[index - 1] + raw_range_delta + doppler_smoothed[index] = ( + prediction + alpha * (raw_code[index] - prediction) + ) + filtered_prediction = ( + firmware_iir_doppler_smoothed[index - 1] + + filtered_range_delta + ) + firmware_iir_doppler_smoothed[index] = ( + filtered_prediction + + alpha * (raw_code[index] - filtered_prediction) + ) + + warm = time_s >= 5.0 + dynamic = ((time_s >= 20.0) & (time_s < 25.0)) | ( + (time_s >= 45.0) & (time_s < 50.0) + ) + rmse = lambda error, mask: float(np.sqrt(np.mean(np.square(error[mask])))) + return SmoothingResult( + raw_rmse_m=rmse(raw_code - true_range, warm), + scalar_iir_rmse_m=rmse(scalar - true_range, warm), + doppler_smoothed_rmse_m=rmse(doppler_smoothed - true_range, warm), + firmware_iir_doppler_smoothed_rmse_m=rmse( + firmware_iir_doppler_smoothed - true_range, + warm, + ), + scalar_iir_peak_dynamic_error_m=float( + np.max(np.abs(scalar[dynamic] - true_range[dynamic])) + ), + doppler_peak_dynamic_error_m=float( + np.max(np.abs(doppler_smoothed[dynamic] - true_range[dynamic])) + ), + firmware_iir_doppler_peak_dynamic_error_m=float( + np.max( + np.abs( + firmware_iir_doppler_smoothed[dynamic] + - true_range[dynamic] + ) + ) + ), + ) diff --git a/soc/software/gps_measurement_python/doppler_eskf_model.py b/soc/software/gps_measurement_python/doppler_eskf_model.py new file mode 100644 index 0000000..54849fd --- /dev/null +++ b/soc/software/gps_measurement_python/doppler_eskf_model.py @@ -0,0 +1,657 @@ +"""Project-specific Doppler WLS to ESKF timing/noise model. + +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; +* a global IMU-driven adaptive-alpha IIR; +* raw Doppler with the per-satellite robust WLS gate used before the ESKF. + +The ESKF is reduced to the velocity and accelerometer-bias error states that +matter to this experiment. Its process noise, 3-D measurement covariance and +99.9-percent velocity NIS gate mirror the project firmware. Attitude and +position are intentionally outside this model's scope. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from enum import Enum +from typing import Callable + +import numpy as np + + +IMU_HZ = 100.0 +TRACKING_HZ = 50.0 +VELOCITY_WLS_HZ = 5.0 +IMU_DT_S = 1.0 / IMU_HZ +TRACKING_STEPS = int(round(IMU_HZ / TRACKING_HZ)) +WLS_STEPS = int(round(IMU_HZ / VELOCITY_WLS_HZ)) +FIXED_DOPPLER_ALPHA = 0.05 +CN0_ALPHA = 0.20 +VELOCITY_NIS_GATE = 16.26623619623813 +PROCESS_ACCEL_NOISE_DENSITY = 0.025 +ACCEL_BIAS_RANDOM_WALK = 0.0015 +MINIMUM_HORIZONTAL_VELOCITY_STD_MPS = 2.0 +MINIMUM_VERTICAL_VELOCITY_STD_MPS = 3.0 + + +class DopplerPath(str, Enum): + FIXED_IIR = "fixed_iir_wls_eskf" + RAW = "raw_wls_eskf" + ADAPTIVE_IIR = "adaptive_iir_wls_eskf" + RAW_ROBUST = "raw_robust_wls_eskf" + + +PATH_ORDER = ( + DopplerPath.FIXED_IIR, + DopplerPath.RAW, + DopplerPath.ADAPTIVE_IIR, + DopplerPath.RAW_ROBUST, +) + + +@dataclass(frozen=True) +class FlightScenario: + name: str + description: str + duration_s: float + initial_velocity_mps: np.ndarray + acceleration: Callable[[float], np.ndarray] + cn0_offset: Callable[[float], float] + noise_multiplier: Callable[[float, int], float] + outlier_mps: Callable[[float, int], float] + receiver_clock_drift_mps: Callable[[float], float] + gps_gaps: tuple[tuple[float, float], ...] = () + + +@dataclass(frozen=True) +class VelocityWlsResult: + velocity_mps: np.ndarray + clock_drift_mps: float + covariance_mps2: np.ndarray + residual_mps: np.ndarray + residual_rms_mps: float + covariance_scale: float + excluded_satellite: int | None + + +@dataclass +class AdaptiveAlpha: + alpha: float = FIXED_DOPPLER_ALPHA + hold_samples: int = 0 + filtered_acceleration_mps2: np.ndarray = field( + default_factory=lambda: np.zeros(3, dtype=float) + ) + + def update( + self, + measured_acceleration_mps2: np.ndarray, + median_cn0_dbhz: float, + ) -> float: + # A raw 100 Hz accelerometer sample is too noisy for coefficient + # switching. This 50 Hz vector EWMA has an approximately 0.16 s time + # constant and, unlike smoothing |a|, does not retain the positive + # magnitude bias of zero-mean sensor noise. + motion_alpha = 0.12 + self.filtered_acceleration_mps2 += motion_alpha * ( + measured_acceleration_mps2 - self.filtered_acceleration_mps2 + ) + magnitude = float(np.linalg.norm(self.filtered_acceleration_mps2)) + + if median_cn0_dbhz >= 46.0: + quiet_alpha = 0.08 + elif median_cn0_dbhz >= 37.0: + quiet_alpha = FIXED_DOPPLER_ALPHA + else: + quiet_alpha = 0.03 + + if magnitude >= 2.5: + self.alpha = 0.40 + self.hold_samples = 25 + elif magnitude >= 0.55: + self.alpha = 0.20 + self.hold_samples = 25 + elif self.hold_samples > 0: + self.hold_samples -= 1 + else: + self.alpha = quiet_alpha + return self.alpha + + +@dataclass +class VelocityBiasEskf: + velocity_mps: np.ndarray + accel_bias_mps2: np.ndarray + covariance: np.ndarray + accepted: int = 0 + rejected_nis: int = 0 + last_nis: float = float("nan") + + @classmethod + def initialize( + cls, + initial_velocity_mps: np.ndarray, + initial_velocity_error_mps: np.ndarray, + ) -> "VelocityBiasEskf": + covariance = np.zeros((6, 6), dtype=float) + covariance[:3, :3] = np.eye(3) + covariance[3:, 3:] = np.eye(3) * 0.1**2 + return cls( + velocity_mps=initial_velocity_mps + initial_velocity_error_mps, + accel_bias_mps2=np.zeros(3), + covariance=covariance, + ) + + def propagate(self, measured_acceleration_mps2: np.ndarray, dt_s: float) -> None: + self.velocity_mps += ( + measured_acceleration_mps2 - self.accel_bias_mps2 + ) * dt_s + + transition = np.eye(6) + transition[:3, 3:] = -np.eye(3) * dt_s + accel_variance = PROCESS_ACCEL_NOISE_DENSITY**2 + bias_variance = ACCEL_BIAS_RANDOM_WALK**2 + process = np.zeros((6, 6), dtype=float) + # Match eskf.c's first-order discrete injection: velocity and bias + # driving noise are added after F P F^T, without exact-discretization + # dt^2/dt^3 cross terms. + process[:3, :3] = np.eye(3) * accel_variance * dt_s + process[3:, 3:] = np.eye(3) * bias_variance * dt_s + self.covariance = ( + transition @ self.covariance @ transition.T + process + ) + self.covariance = 0.5 * (self.covariance + self.covariance.T) + + def update_velocity( + self, + measurement_mps: np.ndarray, + covariance_mps2: np.ndarray, + ) -> bool: + innovation = measurement_mps - self.velocity_mps + innovation_covariance = self.covariance[:3, :3] + covariance_mps2 + inverse = np.linalg.inv(innovation_covariance) + nis = float(innovation @ inverse @ innovation) + self.last_nis = nis + if not np.isfinite(nis) or nis > VELOCITY_NIS_GATE: + self.rejected_nis += 1 + return False + + measurement_matrix = np.zeros((3, 6), dtype=float) + measurement_matrix[:, :3] = np.eye(3) + gain = self.covariance @ measurement_matrix.T @ inverse + correction = gain @ innovation + self.velocity_mps += correction[:3] + self.accel_bias_mps2 += correction[3:] + identity = np.eye(6) + residual_transform = identity - gain @ measurement_matrix + self.covariance = ( + residual_transform + @ self.covariance + @ residual_transform.T + + gain @ covariance_mps2 @ gain.T + ) + self.covariance = 0.5 * (self.covariance + self.covariance.T) + self.accepted += 1 + return True + + +@dataclass(frozen=True) +class DopplerEskfTrace: + scenario: str + path: DopplerPath + time_s: np.ndarray + truth_velocity_mps: np.ndarray + fused_velocity_mps: np.ndarray + fused_velocity_covariance_mps2: np.ndarray + acceleration_mps2: np.ndarray + wls_time_s: np.ndarray + wls_velocity_mps: np.ndarray + wls_clock_drift_mps: np.ndarray + wls_covariance_mps2: np.ndarray + wls_covariance_scale: np.ndarray + wls_nis: np.ndarray + wls_accepted: np.ndarray + adaptive_alpha: np.ndarray + excluded_satellites: np.ndarray + + +def _zero_acceleration(_time_s: float) -> np.ndarray: + return np.zeros(3) + + +def _airliner_acceleration(time_s: float) -> np.ndarray: + if 15.0 <= time_s < 25.0: + return np.asarray([2.0, 0.0, 0.0]) + if 38.0 <= time_s < 52.0: + return np.asarray([0.0, 1.5, 0.25]) + if 68.0 <= time_s < 76.0: + return np.asarray([-2.5, 0.0, -0.15]) + return np.zeros(3) + + +def _aggressive_acceleration(time_s: float) -> np.ndarray: + if 10.0 <= time_s < 16.0: + return np.asarray([6.0, 0.0, 1.0]) + if 26.0 <= time_s < 33.0: + return np.asarray([0.0, 7.0, 0.0]) + if 46.0 <= time_s < 52.0: + return np.asarray([-5.0, -4.0, -1.0]) + if 64.0 <= time_s < 76.0: + phase = 2.0 * np.pi * (time_s - 64.0) / 3.0 + return np.asarray([3.0 * np.sin(phase), 4.0 * np.cos(phase), 0.5 * np.sin(phase)]) + return np.zeros(3) + + +def _constant_zero(_time_s: float) -> float: + return 0.0 + + +def _constant_one(_time_s: float, _satellite: int) -> float: + return 1.0 + + +def _no_outlier(_time_s: float, _satellite: int) -> float: + return 0.0 + + +def flight_scenarios() -> tuple[FlightScenario, ...]: + return ( + FlightScenario( + "steady_cruise", + "Constant aircraft velocity with nominal open-sky Doppler noise.", + 90.0, + np.asarray([180.0, 20.0, 0.0]), + _zero_acceleration, + _constant_zero, + _constant_one, + _no_outlier, + _constant_zero, + ), + FlightScenario( + "airliner_maneuver", + "Moderate longitudinal, turn and climb-rate accelerations.", + 90.0, + np.asarray([120.0, 0.0, 0.0]), + _airliner_acceleration, + _constant_zero, + _constant_one, + _no_outlier, + _constant_zero, + ), + FlightScenario( + "aggressive_maneuver", + "High acceleration and oscillatory manoeuvre segments.", + 90.0, + np.asarray([80.0, 0.0, 0.0]), + _aggressive_acceleration, + _constant_zero, + _constant_one, + _no_outlier, + _constant_zero, + ), + FlightScenario( + "low_cn0_airliner", + "Airliner manoeuvre with a 10 dB signal-quality loss.", + 90.0, + np.asarray([120.0, 0.0, 0.0]), + _airliner_acceleration, + lambda _time_s: -10.0, + _constant_one, + _no_outlier, + _constant_zero, + ), + FlightScenario( + "single_satellite_outliers", + "One channel has repeatable 12 m/s Doppler bursts.", + 90.0, + np.asarray([120.0, 0.0, 0.0]), + _airliner_acceleration, + _constant_zero, + _constant_one, + lambda time_s, satellite: ( + 12.0 + if satellite == 2 + and ( + 30.0 <= time_s < 31.0 + or 56.0 <= time_s < 57.0 + or 72.0 <= time_s < 72.6 + ) + else 0.0 + ), + _constant_zero, + ), + FlightScenario( + "clock_drift_steps", + "Common receiver clock-frequency steps coexist with manoeuvres.", + 90.0, + np.asarray([120.0, 0.0, 0.0]), + _airliner_acceleration, + _constant_zero, + _constant_one, + _no_outlier, + lambda time_s: ( + 18.0 if 28.0 <= time_s < 55.0 else (-9.0 if time_s >= 55.0 else 0.0) + ), + ), + FlightScenario( + "gps_gap_recovery", + "Fifteen-second GNSS gap during an aircraft manoeuvre.", + 90.0, + np.asarray([120.0, 0.0, 0.0]), + _airliner_acceleration, + _constant_zero, + _constant_one, + _no_outlier, + _constant_zero, + gps_gaps=((42.0, 57.0),), + ), + FlightScenario( + "mixed_flight_faults", + "Aggressive motion, temporary weak signal, channel bursts and clock step.", + 90.0, + np.asarray([80.0, 0.0, 0.0]), + _aggressive_acceleration, + lambda time_s: -9.0 if 34.0 <= time_s < 60.0 else 0.0, + lambda time_s, satellite: ( + 2.5 if 34.0 <= time_s < 60.0 and satellite in (1, 6) else 1.0 + ), + lambda time_s, satellite: ( + -10.0 if satellite == 4 and 54.0 <= time_s < 55.0 else 0.0 + ), + lambda time_s: 12.0 if time_s >= 48.0 else 0.0, + gps_gaps=((78.0, 83.0),), + ), + ) + + +def local_los_matrix() -> np.ndarray: + azimuth_deg = np.asarray([8.0, 55.0, 104.0, 151.0, 205.0, 253.0, 301.0, 342.0]) + elevation_deg = np.asarray([73.0, 48.0, 31.0, 18.0, 64.0, 39.0, 24.0, 14.0]) + azimuth = np.deg2rad(azimuth_deg) + elevation = np.deg2rad(elevation_deg) + return np.column_stack( + [ + np.cos(elevation) * np.sin(azimuth), + np.cos(elevation) * np.cos(azimuth), + np.sin(elevation), + ] + ) + + +def velocity_sigma_mps(cn0_dbhz: np.ndarray) -> np.ndarray: + return np.clip(30.0 * np.power(10.0, -cn0_dbhz / 20.0), 0.05, 5.0) + + +def _solve_velocity_candidate( + design: np.ndarray, + observations_mps: np.ndarray, + sigma_mps: np.ndarray, + indices: np.ndarray, +) -> VelocityWlsResult: + selected_design = design[indices] + selected_observations = observations_mps[indices] + selected_sigma = sigma_mps[indices] + weights = 1.0 / np.square(selected_sigma) + normal = selected_design.T @ (weights[:, None] * selected_design) + inverse = np.linalg.inv(normal) + solution = inverse @ (selected_design.T @ (weights * selected_observations)) + residual = selected_observations - selected_design @ solution + weighted_residual_square_sum = float(np.sum(weights * np.square(residual))) + degrees_of_freedom = len(indices) - 4 + covariance_scale = ( + max(1.0, weighted_residual_square_sum / degrees_of_freedom) + if degrees_of_freedom > 0 + else 1.0 + ) + return VelocityWlsResult( + velocity_mps=solution[:3].copy(), + clock_drift_mps=float(solution[3]), + covariance_mps2=covariance_scale * inverse[:3, :3], + residual_mps=residual, + residual_rms_mps=float(np.sqrt(np.mean(np.square(residual)))), + covariance_scale=covariance_scale, + excluded_satellite=None, + ) + + +def velocity_wls( + observations_mps: np.ndarray, + cn0_dbhz: np.ndarray, + *, + robust: bool = False, +) -> VelocityWlsResult: + los = local_los_matrix() + design = np.column_stack([-los, np.ones(len(los))]) + sigma = velocity_sigma_mps(cn0_dbhz) + indices = np.arange(len(observations_mps), dtype=int) + full = _solve_velocity_candidate(design, observations_mps, sigma, indices) + if not robust or len(indices) < 6: + return full + + normalized = np.abs(full.residual_mps) / sigma + if float(np.max(normalized)) <= 3.5: + return full + + # The largest full-fit residual is not necessarily the bad satellite: + # WLS leverage can distribute one channel's error over the other rows. + # Compare every leave-one-out hypothesis and use the one with the lowest + # normalized post-fit residual energy. The firmware robust velocity WLS + # uses the same trigger and candidate selection. + candidates: list[tuple[float, int, VelocityWlsResult]] = [] + for excluded in indices: + used = indices[indices != excluded] + candidate = _solve_velocity_candidate(design, observations_mps, sigma, used) + score = float( + np.sum(np.square(candidate.residual_mps / sigma[used])) + ) + candidates.append((score, int(excluded), candidate)) + _score, excluded, candidate = min(candidates, key=lambda item: item[0]) + return VelocityWlsResult( + **{ + **candidate.__dict__, + "excluded_satellite": excluded, + } + ) + + +def fusion_velocity_covariance(covariance_mps2: np.ndarray) -> np.ndarray: + """Apply fusion_gps_adapter.c's ENU velocity covariance sanitizer floors.""" + + output = np.asarray(covariance_mps2, dtype=float).copy() + variance_floor = np.square( + np.asarray( + [ + MINIMUM_HORIZONTAL_VELOCITY_STD_MPS, + MINIMUM_HORIZONTAL_VELOCITY_STD_MPS, + MINIMUM_VERTICAL_VELOCITY_STD_MPS, + ] + ) + ) + for axis in range(3): + output[axis, axis] = max(output[axis, axis], variance_floor[axis]) + for row in range(3): + for column in range(row + 1, 3): + covariance = 0.5 * (output[row, column] + output[column, row]) + limit = 0.95 * np.sqrt(output[row, row] * output[column, column]) + covariance = float(np.clip(covariance, -limit, limit)) + output[row, column] = covariance + output[column, row] = covariance + return output + + +def _in_gap(scenario: FlightScenario, time_s: float) -> bool: + return any(start <= time_s < stop for start, stop in scenario.gps_gaps) + + +def _truth_trajectory(scenario: FlightScenario) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + time_s = np.arange(0.0, scenario.duration_s, IMU_DT_S) + acceleration = np.asarray([scenario.acceleration(float(time)) for time in time_s]) + velocity = np.empty((len(time_s), 3), dtype=float) + velocity[0] = scenario.initial_velocity_mps + velocity[1:] = velocity[0] + np.cumsum(acceleration[:-1] * IMU_DT_S, axis=0) + return time_s, acceleration, velocity + + +def run_doppler_eskf_scenario( + scenario: FlightScenario, + seed: int, + paths: tuple[DopplerPath, ...] = PATH_ORDER, +) -> dict[DopplerPath, DopplerEskfTrace]: + rng = np.random.default_rng(seed) + time_s, truth_acceleration, truth_velocity = _truth_trajectory(scenario) + satellite_count = len(local_los_matrix()) + baseline_cn0 = np.asarray([47.0, 45.0, 43.0, 38.0, 46.0, 42.0, 40.0, 36.0]) + filtered_cn0 = baseline_cn0.copy() + fixed_measurement: np.ndarray | None = None + adaptive_measurement: np.ndarray | None = None + latest_raw: np.ndarray | None = None + alpha_selector = AdaptiveAlpha() + latest_alpha = FIXED_DOPPLER_ALPHA + + initial_error = rng.normal(0.0, 0.8, 3) + filters = { + path: VelocityBiasEskf.initialize( + scenario.initial_velocity_mps, + initial_error, + ) + for path in paths + } + fused_history = { + path: np.empty((len(time_s), 3), dtype=float) + for path in paths + } + fused_covariance_history = { + path: np.empty((len(time_s), 3, 3), dtype=float) + for path in paths + } + wls_times = {path: [] for path in paths} + wls_velocity = {path: [] for path in paths} + wls_clock_drift = {path: [] for path in paths} + wls_covariance = {path: [] for path in paths} + wls_covariance_scale = {path: [] for path in paths} + wls_nis = {path: [] for path in paths} + wls_accepted = {path: [] for path in paths} + wls_alpha = {path: [] for path in paths} + excluded = {path: [] for path in paths} + + true_accel_bias = rng.normal(0.0, 0.04, 3) + latest_acceleration_measurement = truth_acceleration[0] + true_accel_bias + los = local_los_matrix() + + for sample in range(len(time_s)): + if sample > 0: + true_accel_bias += rng.normal( + 0.0, + ACCEL_BIAS_RANDOM_WALK * np.sqrt(IMU_DT_S), + 3, + ) + imu_noise_std = PROCESS_ACCEL_NOISE_DENSITY / np.sqrt(IMU_DT_S) + latest_acceleration_measurement = ( + truth_acceleration[sample - 1] + + true_accel_bias + + rng.normal(0.0, imu_noise_std, 3) + ) + for path in paths: + filters[path].propagate(latest_acceleration_measurement, IMU_DT_S) + + if sample % TRACKING_STEPS == 0: + current_time = float(time_s[sample]) + true_cn0 = baseline_cn0 + scenario.cn0_offset(current_time) + raw_cn0 = np.clip(true_cn0 + rng.normal(0.0, 1.2, satellite_count), 10.0, 60.0) + filtered_cn0 = CN0_ALPHA * raw_cn0 + (1.0 - CN0_ALPHA) * filtered_cn0 + nominal_sigma = velocity_sigma_mps(true_cn0) + actual_sigma = nominal_sigma * np.asarray( + [ + scenario.noise_multiplier(current_time, satellite) + for satellite in range(satellite_count) + ] + ) + raw = ( + -los @ truth_velocity[sample] + + scenario.receiver_clock_drift_mps(current_time) + + rng.normal(0.0, actual_sigma) + + np.asarray( + [ + scenario.outlier_mps(current_time, satellite) + for satellite in range(satellite_count) + ] + ) + ) + if fixed_measurement is None: + fixed_measurement = raw.copy() + adaptive_measurement = raw.copy() + else: + fixed_measurement += FIXED_DOPPLER_ALPHA * (raw - fixed_measurement) + latest_alpha = alpha_selector.update( + latest_acceleration_measurement, + float(np.median(filtered_cn0)), + ) + adaptive_measurement += latest_alpha * (raw - adaptive_measurement) + latest_raw = raw + + if ( + sample > 0 + and sample % WLS_STEPS == 0 + and latest_raw is not None + and not _in_gap(scenario, float(time_s[sample])) + ): + for path in paths: + if path == DopplerPath.FIXED_IIR: + observations = fixed_measurement + elif path == DopplerPath.ADAPTIVE_IIR: + observations = adaptive_measurement + else: + observations = latest_raw + result = velocity_wls( + np.asarray(observations), + filtered_cn0, + robust=(path == DopplerPath.RAW_ROBUST), + ) + accepted = filters[path].update_velocity( + result.velocity_mps, + fusion_velocity_covariance(result.covariance_mps2), + ) + wls_times[path].append(float(time_s[sample])) + wls_velocity[path].append(result.velocity_mps) + wls_clock_drift[path].append(result.clock_drift_mps) + wls_covariance[path].append(result.covariance_mps2) + wls_covariance_scale[path].append(result.covariance_scale) + wls_nis[path].append(filters[path].last_nis) + wls_accepted[path].append(accepted) + wls_alpha[path].append( + latest_alpha + if path == DopplerPath.ADAPTIVE_IIR + else (FIXED_DOPPLER_ALPHA if path == DopplerPath.FIXED_IIR else 1.0) + ) + excluded[path].append( + -1 if result.excluded_satellite is None else result.excluded_satellite + ) + + for path in paths: + fused_history[path][sample] = filters[path].velocity_mps + fused_covariance_history[path][sample] = filters[path].covariance[:3, :3] + + return { + path: DopplerEskfTrace( + scenario=scenario.name, + path=path, + time_s=time_s, + truth_velocity_mps=truth_velocity, + fused_velocity_mps=fused_history[path], + fused_velocity_covariance_mps2=fused_covariance_history[path], + acceleration_mps2=truth_acceleration, + wls_time_s=np.asarray(wls_times[path]), + wls_velocity_mps=np.asarray(wls_velocity[path]), + wls_clock_drift_mps=np.asarray(wls_clock_drift[path]), + wls_covariance_mps2=np.asarray(wls_covariance[path]), + wls_covariance_scale=np.asarray(wls_covariance_scale[path]), + wls_nis=np.asarray(wls_nis[path]), + wls_accepted=np.asarray(wls_accepted[path], dtype=bool), + adaptive_alpha=np.asarray(wls_alpha[path]), + excluded_satellites=np.asarray(excluded[path], dtype=int), + ) + for path in paths + } diff --git a/soc/software/gps_measurement_python/firmware_wls_model.py b/soc/software/gps_measurement_python/firmware_wls_model.py new file mode 100644 index 0000000..2beb8a0 --- /dev/null +++ b/soc/software/gps_measurement_python/firmware_wls_model.py @@ -0,0 +1,353 @@ +"""Python mirror of the firmware's code-measurement WLS policy. + +The model copies the receiver-specific parts of NavEquations.c: + +* four states: ECEF x/y/z and common receiver clock bias in metres; +* URA + elevation + C/N0 variance model; +* normalized weights for the numerical solve and absolute weights for + covariance/integrity; +* the 99-percent chi-square table; +* single-satellite leave-one-out FDE only when at least seven measurements + are available. + +Atmospheric and satellite-clock corrections are represented as already +corrected measurement errors. That keeps this study focused on tracking +quality rather than duplicating the existing PPS orbit/atmosphere model. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum + +import numpy as np + + +URA_SIGMA_M = np.asarray( + [ + 2.4, + 3.4, + 4.85, + 6.85, + 9.65, + 13.65, + 24.0, + 48.0, + 96.0, + 192.0, + 384.0, + 768.0, + 1536.0, + 3072.0, + 6144.0, + ], + dtype=float, +) +CHI_SQUARE_99 = np.asarray( + [0.0, 6.634896601, 9.210340372, 11.34486673, 13.27670414, 15.08627247], + dtype=float, +) + + +class SigmaMethod(str, Enum): + CURRENT = "current_cn0" + NAIVE_ADD_DLL = "naive_add_dll" + DLL_EXCESS = "dll_excess" + DLL_EXCESS_GUARD = "dll_excess_guard" + ORACLE = "oracle_variance" + + +@dataclass(frozen=True) +class SigmaInputs: + ura_index: np.ndarray + elevation_deg: np.ndarray + cn0_dbhz: np.ndarray + dll_variance_ratio: np.ndarray + dll_mean_z: np.ndarray + actual_jitter_multiplier: np.ndarray + + +@dataclass(frozen=True) +class WlsSolution: + position_ecef_m: np.ndarray + clock_bias_m: float + residual_m: np.ndarray + sigma_m: np.ndarray + covariance: np.ndarray + chi_square: float + chi_square_ratio: float + chi_square_pass: bool + valid: bool + used_indices: np.ndarray + excluded_index: int | None + + +def cn0_code_sigma_m(cn0_dbhz: np.ndarray | float) -> np.ndarray: + sigma = 3.0 * np.power(10.0, (45.0 - np.asarray(cn0_dbhz)) / 20.0) + return np.clip(sigma, 0.5, 30.0) + + +def elevation_sigma_m(elevation_deg: np.ndarray | float) -> np.ndarray: + bounded = np.clip(np.asarray(elevation_deg), 10.0, 90.0) + return 3.0 / np.sin(np.deg2rad(bounded)) + + +def firmware_sigma_m( + inputs: SigmaInputs, + method: SigmaMethod, +) -> np.ndarray: + ura = URA_SIGMA_M[np.asarray(inputs.ura_index, dtype=int)] + elevation = elevation_sigma_m(inputs.elevation_deg) + code = cn0_code_sigma_m(inputs.cn0_dbhz) + ratio = np.clip(np.asarray(inputs.dll_variance_ratio), 0.05, 25.0) + + if method == SigmaMethod.CURRENT: + code_variance_factor = np.ones_like(code) + elif method == SigmaMethod.NAIVE_ADD_DLL: + # Deliberately included as a negative control: at ratio=1 this counts + # the C/N0-predicted thermal jitter twice. + code_variance_factor = 1.0 + ratio + elif method == SigmaMethod.DLL_EXCESS: + # Only the measured excess over the C/N0 expectation is added. + code_variance_factor = np.maximum(1.0, ratio) + elif method == SigmaMethod.DLL_EXCESS_GUARD: + mean_factor = np.square( + np.maximum(1.0, np.asarray(inputs.dll_mean_z) / 3.0) + ) + code_variance_factor = np.maximum.reduce( + [np.ones_like(code), ratio, mean_factor] + ) + elif method == SigmaMethod.ORACLE: + code_variance_factor = np.square( + np.maximum(0.05, np.asarray(inputs.actual_jitter_multiplier)) + ) + else: + raise ValueError(f"unsupported sigma method: {method}") + + return np.sqrt( + ura * ura + + elevation * elevation + + code * code * code_variance_factor + ) + + +def ecef_to_lla(ecef_m: np.ndarray) -> tuple[float, float, float]: + x, y, z = np.asarray(ecef_m, dtype=float) + a = 6_378_137.0 + inverse_flattening = 298.257223563 + flattening = 1.0 / inverse_flattening + eccentricity_squared = flattening * (2.0 - flattening) + longitude = np.arctan2(y, x) + horizontal = np.hypot(x, y) + latitude = np.arctan2( + z, + horizontal * (1.0 - eccentricity_squared), + ) + height = 0.0 + for _ in range(12): + sine = np.sin(latitude) + prime_vertical = a / np.sqrt(1.0 - eccentricity_squared * sine * sine) + height = horizontal / max(np.cos(latitude), 1.0e-15) - prime_vertical + next_latitude = np.arctan2( + z, + horizontal + * ( + 1.0 + - eccentricity_squared + * prime_vertical + / max(prime_vertical + height, 1.0) + ), + ) + if abs(next_latitude - latitude) < 1.0e-14: + latitude = next_latitude + break + latitude = next_latitude + return ( + float(np.rad2deg(latitude)), + float(np.rad2deg(longitude)), + float(height), + ) + + +def lla_to_ecef(latitude_deg: float, longitude_deg: float, height_m: float) -> np.ndarray: + a = 6_378_137.0 + inverse_flattening = 298.257223563 + flattening = 1.0 / inverse_flattening + eccentricity_squared = flattening * (2.0 - flattening) + latitude = np.deg2rad(latitude_deg) + longitude = np.deg2rad(longitude_deg) + sine = np.sin(latitude) + cosine = np.cos(latitude) + prime_vertical = a / np.sqrt(1.0 - eccentricity_squared * sine * sine) + return np.asarray( + [ + (prime_vertical + height_m) * cosine * np.cos(longitude), + (prime_vertical + height_m) * cosine * np.sin(longitude), + ( + prime_vertical * (1.0 - eccentricity_squared) + + height_m + ) + * sine, + ], + dtype=float, + ) + + +def enu_axes(receiver_ecef_m: np.ndarray) -> np.ndarray: + latitude_deg, longitude_deg, _ = ecef_to_lla(receiver_ecef_m) + latitude = np.deg2rad(latitude_deg) + longitude = np.deg2rad(longitude_deg) + east = np.asarray([-np.sin(longitude), np.cos(longitude), 0.0]) + north = np.asarray( + [ + -np.sin(latitude) * np.cos(longitude), + -np.sin(latitude) * np.sin(longitude), + np.cos(latitude), + ] + ) + up = np.asarray( + [ + np.cos(latitude) * np.cos(longitude), + np.cos(latitude) * np.sin(longitude), + np.sin(latitude), + ] + ) + return np.vstack([east, north, up]) + + +def satellites_from_azel( + receiver_ecef_m: np.ndarray, + azimuth_deg: np.ndarray, + elevation_deg: np.ndarray, + range_m: float = 21_000_000.0, +) -> np.ndarray: + axes = enu_axes(receiver_ecef_m) + satellites: list[np.ndarray] = [] + for azimuth, elevation in zip(azimuth_deg, elevation_deg): + azimuth_rad = np.deg2rad(azimuth) + elevation_rad = np.deg2rad(elevation) + los_enu = np.asarray( + [ + np.cos(elevation_rad) * np.sin(azimuth_rad), + np.cos(elevation_rad) * np.cos(azimuth_rad), + np.sin(elevation_rad), + ] + ) + los_ecef = axes.T @ los_enu + satellites.append(receiver_ecef_m + range_m * los_ecef) + return np.asarray(satellites) + + +def position_error_enu_m( + estimated_ecef_m: np.ndarray, + true_ecef_m: np.ndarray, +) -> np.ndarray: + return enu_axes(true_ecef_m) @ (estimated_ecef_m - true_ecef_m) + + +def _candidate( + satellite_positions_m: np.ndarray, + observations_m: np.ndarray, + sigma_m: np.ndarray, + initial_state: np.ndarray, + used_indices: np.ndarray, +) -> WlsSolution: + state = np.asarray(initial_state, dtype=float).copy() + weights = 1.0 / np.square(sigma_m) + design = np.empty((len(observations_m), 4), dtype=float) + + for _ in range(10): + difference = satellite_positions_m - state[:3] + geometric_range = np.linalg.norm(difference, axis=1) + design[:, :3] = -difference / geometric_range[:, None] + design[:, 3] = 1.0 + residual = observations_m - geometric_range - state[3] + normal = design.T @ (weights[:, None] * design) + rhs = design.T @ (weights * residual) + update = np.linalg.solve(normal, rhs) + state += update + if np.linalg.norm(update) < 1.0e-4: + break + + difference = satellite_positions_m - state[:3] + geometric_range = np.linalg.norm(difference, axis=1) + design[:, :3] = -difference / geometric_range[:, None] + design[:, 3] = 1.0 + residual = observations_m - geometric_range - state[3] + normal = design.T @ (weights[:, None] * design) + covariance = np.linalg.inv(normal) + chi_square = float(np.sum(weights * np.square(residual))) + degrees_of_freedom = len(observations_m) - 4 + threshold = ( + float(CHI_SQUARE_99[degrees_of_freedom]) + if 0 < degrees_of_freedom < len(CHI_SQUARE_99) + else 0.0 + ) + chi_square_pass = degrees_of_freedom > 0 and chi_square <= threshold + valid = ( + (len(observations_m) == 4 and degrees_of_freedom == 0) + or chi_square_pass + ) + ratio = chi_square / threshold if threshold > 0.0 else float("inf") + return WlsSolution( + position_ecef_m=state[:3].copy(), + clock_bias_m=float(state[3]), + residual_m=residual.copy(), + sigma_m=sigma_m.copy(), + covariance=covariance, + chi_square=chi_square, + chi_square_ratio=ratio, + chi_square_pass=chi_square_pass, + valid=valid, + used_indices=used_indices.copy(), + excluded_index=None, + ) + + +def firmware_wls( + satellite_positions_m: np.ndarray, + observations_m: np.ndarray, + sigma_m: np.ndarray, + initial_state: np.ndarray, + *, + enable_fde: bool = True, +) -> WlsSolution: + count = len(observations_m) + if count < 4: + raise ValueError("position WLS needs at least four satellites") + full_indices = np.arange(count, dtype=int) + full = _candidate( + satellite_positions_m, + observations_m, + sigma_m, + initial_state, + full_indices, + ) + if full.chi_square_pass or not enable_fde or count < 7: + return full + + selected = full + passing_found = False + full_state = np.concatenate([full.position_ecef_m, [full.clock_bias_m]]) + for excluded in range(count): + used = full_indices[full_indices != excluded] + candidate = _candidate( + satellite_positions_m[used], + observations_m[used], + sigma_m[used], + full_state, + used, + ) + candidate = WlsSolution( + **{ + **candidate.__dict__, + "excluded_index": excluded, + } + ) + if candidate.chi_square_pass: + if not passing_found or candidate.chi_square_ratio < selected.chi_square_ratio: + selected = candidate + passing_found = True + elif not passing_found and candidate.chi_square_ratio < selected.chi_square_ratio: + selected = candidate + return selected diff --git a/soc/software/gps_measurement_python/measurement_scenarios.py b/soc/software/gps_measurement_python/measurement_scenarios.py new file mode 100644 index 0000000..706740f --- /dev/null +++ b/soc/software/gps_measurement_python/measurement_scenarios.py @@ -0,0 +1,368 @@ +"""Synthetic code-tracking scenarios shared by the analysis and tests.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Callable + +import numpy as np + +from .firmware_wls_model import ( + SigmaInputs, + SigmaMethod, + URA_SIGMA_M, + cn0_code_sigma_m, + elevation_sigma_m, + enu_axes, + firmware_sigma_m, + firmware_wls, + lla_to_ecef, + position_error_enu_m, + satellites_from_azel, +) +from .rtl_dll_model import expected_discriminator_std, synthesize_dll_window + + +TRACKING_PERIOD_S = 0.020 +WLS_PERIOD_S = 0.200 +TRACKING_UPDATES_PER_WLS = int(round(WLS_PERIOD_S / TRACKING_PERIOD_S)) +FIRMWARE_CN0_ALPHA = 0.20 +DLL_RATIO_ALPHA = 0.10 +DLL_MEAN_ALPHA = 0.20 + +GOOD_AZIMUTH_DEG = np.asarray([8.0, 55.0, 104.0, 151.0, 205.0, 253.0, 301.0, 342.0]) +GOOD_ELEVATION_DEG = np.asarray([73.0, 48.0, 31.0, 18.0, 64.0, 39.0, 24.0, 14.0]) +POOR_AZIMUTH_DEG = np.asarray([25.0, 46.0, 68.0, 89.0, 112.0, 136.0, 159.0, 182.0]) +POOR_ELEVATION_DEG = np.asarray([18.0, 22.0, 25.0, 29.0, 32.0, 27.0, 21.0, 16.0]) + + +Profile = Callable[[float, int, float], float] + + +def _identity(_time_s: float, _satellite: int, baseline: float) -> float: + return baseline + + +def _one(_time_s: float, _satellite: int, _baseline: float) -> float: + return 1.0 + + +def _zero(_time_s: float, _satellite: int, _baseline: float) -> float: + return 0.0 + + +@dataclass(frozen=True) +class Scenario: + name: str + description: str + duration_s: float = 80.0 + poor_geometry: bool = False + cn0_profile: Profile = _identity + jitter_profile: Profile = _one + discriminator_bias_profile: Profile = _zero + pseudorange_bias_profile: Profile = _zero + + +@dataclass +class ChannelQuality: + cn0_dbhz: float | None = None + dll_ratio: float = 1.0 + dll_mean_z: float = 0.0 + + def update( + self, + raw_cn0_dbhz: float, + dll_samples: np.ndarray, + ) -> None: + if self.cn0_dbhz is None: + self.cn0_dbhz = raw_cn0_dbhz + else: + self.cn0_dbhz = ( + FIRMWARE_CN0_ALPHA * raw_cn0_dbhz + + (1.0 - FIRMWARE_CN0_ALPHA) * self.cn0_dbhz + ) + + measured_variance = float(np.var(dll_samples, ddof=1)) + expected_std = expected_discriminator_std(self.cn0_dbhz) + raw_ratio = measured_variance / max(expected_std * expected_std, 1.0e-15) + raw_ratio = float(np.clip(raw_ratio, 0.05, 25.0)) + self.dll_ratio = ( + DLL_RATIO_ALPHA * raw_ratio + + (1.0 - DLL_RATIO_ALPHA) * self.dll_ratio + ) + + standard_error = np.sqrt( + max(measured_variance, expected_std * expected_std) + / len(dll_samples) + ) + raw_mean_z = abs(float(np.mean(dll_samples))) / max(standard_error, 1.0e-12) + raw_mean_z = float(np.clip(raw_mean_z, 0.0, 12.0)) + self.dll_mean_z = ( + DLL_MEAN_ALPHA * raw_mean_z + + (1.0 - DLL_MEAN_ALPHA) * self.dll_mean_z + ) + + +@dataclass(frozen=True) +class EpochSample: + method: SigmaMethod + time_s: float + enu_error_m: np.ndarray + horizontal_error_m: float + position_error_3d_m: float + horizontal_sigma_m: float + valid: bool + chi_square_ratio: float + fde_applied: bool + mean_measurement_sigma_m: float + maximum_dll_ratio: float + + +def scenario_matrix() -> tuple[Scenario, ...]: + return ( + Scenario( + "clean_open_sky", + "C/N0 model and actual DLL jitter agree.", + ), + Scenario( + "weak_signal", + "All channels lose 11 dB; current C/N0 weighting already sees it.", + cn0_profile=lambda _t, _s, baseline: baseline - 11.0, + ), + Scenario( + "one_channel_hidden_jitter", + "One high-C/N0 channel has 4x DLL jitter not visible in C/N0.", + jitter_profile=lambda _t, satellite, _b: 4.0 if satellite == 1 else 1.0, + ), + Scenario( + "common_hidden_jitter", + "All channels have the same unmodelled 3x DLL jitter.", + jitter_profile=lambda _t, _satellite, _b: 3.0, + ), + Scenario( + "noise_burst_recovery", + "Two channels have 5x jitter from 22 s to 45 s, then recover.", + jitter_profile=lambda time_s, satellite, _b: ( + 5.0 + if 22.0 <= time_s < 45.0 and satellite in (1, 5) + else 1.0 + ), + ), + Scenario( + "dll_transient_stress", + "A channel has discriminator mean without excess variance during transient stress.", + discriminator_bias_profile=lambda time_s, satellite, _b: ( + 0.055 + if 25.0 <= time_s < 34.0 and satellite == 3 + else 0.0 + ), + pseudorange_bias_profile=lambda time_s, satellite, _b: ( + 9.0 + if 25.0 <= time_s < 34.0 and satellite == 3 + else 0.0 + ), + ), + Scenario( + "stable_multipath_bias", + "Low-jitter, high-C/N0 channel has a stable 14 m code bias.", + pseudorange_bias_profile=lambda _t, satellite, _b: ( + 14.0 if satellite == 6 else 0.0 + ), + ), + Scenario( + "mixed_multipath", + "One channel has both a 10 m bias and 3x excess jitter.", + jitter_profile=lambda _t, satellite, _b: 3.0 if satellite == 2 else 1.0, + pseudorange_bias_profile=lambda _t, satellite, _b: ( + 10.0 if satellite == 2 else 0.0 + ), + ), + Scenario( + "poor_geometry_hidden_jitter", + "Clustered geometry plus one channel with unmodelled 4x jitter.", + poor_geometry=True, + jitter_profile=lambda _t, satellite, _b: 4.0 if satellite == 4 else 1.0, + ), + Scenario( + "cn0_step_recovery", + "C/N0 is initially weak and improves; no hidden DLL noise exists.", + cn0_profile=lambda time_s, _satellite, baseline: ( + baseline - 12.0 if time_s < 30.0 else baseline + ), + ), + ) + + +def _geometry(scenario: Scenario) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + receiver = lla_to_ecef(39.9208, 32.8541, 930.0) + if scenario.poor_geometry: + azimuth = POOR_AZIMUTH_DEG + elevation = POOR_ELEVATION_DEG + else: + azimuth = GOOD_AZIMUTH_DEG + elevation = GOOD_ELEVATION_DEG + satellites = satellites_from_azel(receiver, azimuth, elevation) + return receiver, satellites, elevation.copy() + + +def run_scenario( + scenario: Scenario, + seed: int, + methods: tuple[SigmaMethod, ...] = tuple(SigmaMethod), +) -> list[EpochSample]: + rng = np.random.default_rng(seed) + receiver, satellites, elevations = _geometry(scenario) + satellite_count = len(satellites) + geometric_ranges = np.linalg.norm(satellites - receiver, axis=1) + baseline_cn0 = np.clip(34.0 + 0.16 * elevations, 30.0, 48.0) + ura_indices = np.zeros(satellite_count, dtype=np.uint8) + quality = [ChannelQuality() for _ in range(satellite_count)] + + # These are deliberately slower/non-DLL terms. They prevent a synthetic + # white-noise-only experiment from overstating what DLL weighting can fix. + ura_error = rng.normal(0.0, 0.45 * URA_SIGMA_M[0], satellite_count) + atmospheric_state = rng.normal( + 0.0, + 0.35 * elevation_sigma_m(elevations), + satellite_count, + ) + code_noise_state = np.zeros(satellite_count) + initial_state = np.concatenate( + [ + receiver + np.asarray([450.0, -300.0, 220.0]), + [0.0], + ] + ) + method_state = {method: initial_state.copy() for method in methods} + samples: list[EpochSample] = [] + clock_bias_m = 82.0 + receiver_enu_axes = enu_axes(receiver) + + tracking_steps = int(round(scenario.duration_s / TRACKING_PERIOD_S)) + latest_actual_jitter = np.ones(satellite_count) + for tracking_step in range(tracking_steps): + time_s = tracking_step * TRACKING_PERIOD_S + for satellite in range(satellite_count): + true_cn0 = scenario.cn0_profile( + time_s, + satellite, + float(baseline_cn0[satellite]), + ) + actual_jitter = scenario.jitter_profile( + time_s, + satellite, + 1.0, + ) + discriminator_bias = scenario.discriminator_bias_profile( + time_s, + satellite, + 0.0, + ) + latest_actual_jitter[satellite] = actual_jitter + dll_window = synthesize_dll_window( + rng, + cn0_dbhz=true_cn0, + jitter_multiplier=actual_jitter, + discriminator_bias=discriminator_bias, + ) + # The hardware NWPR C/N0 estimate is noisy; firmware applies alpha=0.2. + raw_cn0 = float(np.clip(true_cn0 + rng.normal(0.0, 1.3), 10.0, 60.0)) + quality[satellite].update(raw_cn0, dll_window.samples) + + if (tracking_step + 1) % TRACKING_UPDATES_PER_WLS != 0: + continue + + true_cn0 = np.asarray( + [ + scenario.cn0_profile( + time_s, + satellite, + float(baseline_cn0[satellite]), + ) + for satellite in range(satellite_count) + ] + ) + actual_code_sigma = cn0_code_sigma_m(true_cn0) * latest_actual_jitter + innovation = rng.normal(0.0, actual_code_sigma) + code_noise_state = ( + 0.25 * code_noise_state + + np.sqrt(1.0 - 0.25**2) * innovation + ) + atmospheric_innovation = rng.normal( + 0.0, + 0.35 * elevation_sigma_m(elevations), + ) + atmospheric_state = ( + 0.97 * atmospheric_state + + np.sqrt(1.0 - 0.97**2) * atmospheric_innovation + ) + pseudorange_bias = np.asarray( + [ + scenario.pseudorange_bias_profile( + time_s, + satellite, + 0.0, + ) + for satellite in range(satellite_count) + ] + ) + observations = ( + geometric_ranges + + clock_bias_m + + ura_error + + atmospheric_state + + code_noise_state + + pseudorange_bias + ) + sigma_inputs = SigmaInputs( + ura_index=ura_indices, + elevation_deg=elevations, + cn0_dbhz=np.asarray([item.cn0_dbhz for item in quality], dtype=float), + dll_variance_ratio=np.asarray([item.dll_ratio for item in quality]), + dll_mean_z=np.asarray([item.dll_mean_z for item in quality]), + actual_jitter_multiplier=latest_actual_jitter.copy(), + ) + + for method in methods: + sigma = firmware_sigma_m(sigma_inputs, method) + solution = firmware_wls( + satellites, + observations, + sigma, + method_state[method], + ) + # NavEquations.c updates its persistent WLS seed only after an + # integrity-valid solution. + if solution.valid: + method_state[method] = np.concatenate( + [solution.position_ecef_m, [solution.clock_bias_m]] + ) + enu_error = position_error_enu_m(solution.position_ecef_m, receiver) + enu_covariance = ( + # The top-left block is ECEF position covariance. + # R * Q * R' maps it to ENU. + receiver_enu_axes + @ solution.covariance[:3, :3] + @ receiver_enu_axes.T + ) + horizontal_sigma = float( + np.sqrt(max(0.0, enu_covariance[0, 0] + enu_covariance[1, 1])) + ) + samples.append( + EpochSample( + method=method, + time_s=time_s, + enu_error_m=enu_error, + horizontal_error_m=float(np.linalg.norm(enu_error[:2])), + position_error_3d_m=float(np.linalg.norm(enu_error)), + horizontal_sigma_m=horizontal_sigma, + valid=solution.valid, + chi_square_ratio=solution.chi_square_ratio, + fde_applied=solution.excluded_index is not None, + mean_measurement_sigma_m=float(np.mean(sigma)), + maximum_dll_ratio=float( + max(item.dll_ratio for item in quality) + ), + ) + ) + return samples diff --git a/soc/software/gps_measurement_python/output/01_position_3d_rms.png b/soc/software/gps_measurement_python/output/01_position_3d_rms.png new file mode 100644 index 0000000..64db804 Binary files /dev/null and b/soc/software/gps_measurement_python/output/01_position_3d_rms.png differ diff --git a/soc/software/gps_measurement_python/output/02_solution_availability.png b/soc/software/gps_measurement_python/output/02_solution_availability.png new file mode 100644 index 0000000..c976dad Binary files /dev/null and b/soc/software/gps_measurement_python/output/02_solution_availability.png differ diff --git a/soc/software/gps_measurement_python/output/03_noise_burst_transition.png b/soc/software/gps_measurement_python/output/03_noise_burst_transition.png new file mode 100644 index 0000000..37e4ec4 Binary files /dev/null and b/soc/software/gps_measurement_python/output/03_noise_burst_transition.png differ diff --git a/soc/software/gps_measurement_python/output/04_code_smoothing_cfo.png b/soc/software/gps_measurement_python/output/04_code_smoothing_cfo.png new file mode 100644 index 0000000..5606129 Binary files /dev/null and b/soc/software/gps_measurement_python/output/04_code_smoothing_cfo.png differ diff --git a/soc/software/gps_measurement_python/output/05_doppler_eskf_velocity_rms.png b/soc/software/gps_measurement_python/output/05_doppler_eskf_velocity_rms.png new file mode 100644 index 0000000..b34126d Binary files /dev/null and b/soc/software/gps_measurement_python/output/05_doppler_eskf_velocity_rms.png differ diff --git a/soc/software/gps_measurement_python/output/06_airliner_maneuver_trace.png b/soc/software/gps_measurement_python/output/06_airliner_maneuver_trace.png new file mode 100644 index 0000000..62bfd81 Binary files /dev/null and b/soc/software/gps_measurement_python/output/06_airliner_maneuver_trace.png differ diff --git a/soc/software/gps_measurement_python/output/07_outlier_wls_nis.png b/soc/software/gps_measurement_python/output/07_outlier_wls_nis.png new file mode 100644 index 0000000..b6054c3 Binary files /dev/null and b/soc/software/gps_measurement_python/output/07_outlier_wls_nis.png differ diff --git a/soc/software/gps_measurement_python/output/08_low_cn0_wls_vs_eskf.png b/soc/software/gps_measurement_python/output/08_low_cn0_wls_vs_eskf.png new file mode 100644 index 0000000..aaf9f61 Binary files /dev/null and b/soc/software/gps_measurement_python/output/08_low_cn0_wls_vs_eskf.png differ diff --git a/soc/software/gps_measurement_python/output/ANALYSIS.md b/soc/software/gps_measurement_python/output/ANALYSIS.md new file mode 100644 index 0000000..57ff813 --- /dev/null +++ b/soc/software/gps_measurement_python/output/ANALYSIS.md @@ -0,0 +1,94 @@ +# GPS code measurement model sonuçları + +Monte Carlo seed sayısı: `12`. İlk 5 saniye metriklere alınmadı. + +## Yöntem karşılaştırması + +| Senaryo | Yöntem | 3D RMS (m) | Mevcuda göre | P95 (m) | Valid (%) | >25 m false-valid (%) | +|---|---|---:|---:|---:|---:|---:| +| clean_open_sky | Mevcut C/N0 | 10.05 | +0.0% | 17.35 | 100.0 | 0.31 | +| clean_open_sky | Naif DLL ekle | 10.02 | +0.3% | 17.25 | 100.0 | 0.31 | +| clean_open_sky | DLL excess | 10.05 | +0.0% | 17.32 | 100.0 | 0.33 | +| clean_open_sky | DLL excess+mean | 10.05 | +0.0% | 17.32 | 100.0 | 0.33 | +| clean_open_sky | Oracle variance | 10.05 | +0.0% | 17.35 | 100.0 | 0.31 | +| weak_signal | Mevcut C/N0 | 32.73 | +0.0% | 56.44 | 100.0 | 56.31 | +| weak_signal | Naif DLL ekle | 32.31 | +1.3% | 55.46 | 100.0 | 56.18 | +| weak_signal | DLL excess | 32.65 | +0.3% | 56.12 | 100.0 | 56.22 | +| weak_signal | DLL excess+mean | 32.65 | +0.3% | 56.12 | 100.0 | 56.22 | +| weak_signal | Oracle variance | 32.73 | +0.0% | 56.44 | 100.0 | 56.31 | +| one_channel_hidden_jitter | Mevcut C/N0 | 12.27 | +0.0% | 19.94 | 100.0 | 1.07 | +| one_channel_hidden_jitter | Naif DLL ekle | 10.49 | +14.5% | 17.75 | 100.0 | 0.33 | +| one_channel_hidden_jitter | DLL excess | 10.47 | +14.7% | 17.68 | 100.0 | 0.33 | +| one_channel_hidden_jitter | DLL excess+mean | 10.47 | +14.7% | 17.68 | 100.0 | 0.33 | +| one_channel_hidden_jitter | Oracle variance | 10.46 | +14.7% | 17.74 | 100.0 | 0.33 | +| common_hidden_jitter | Mevcut C/N0 | 33.79 | +0.0% | 59.76 | 94.8 | 52.40 | +| common_hidden_jitter | Naif DLL ekle | 27.61 | +18.3% | 47.56 | 100.0 | 43.93 | +| common_hidden_jitter | DLL excess | 27.69 | +18.1% | 47.83 | 100.0 | 43.89 | +| common_hidden_jitter | DLL excess+mean | 27.69 | +18.1% | 47.83 | 100.0 | 43.89 | +| common_hidden_jitter | Oracle variance | 27.74 | +17.9% | 47.83 | 100.0 | 43.87 | +| noise_burst_recovery | Mevcut C/N0 | 12.96 | +0.0% | 22.13 | 98.9 | 2.58 | +| noise_burst_recovery | Naif DLL ekle | 10.52 | +18.8% | 18.03 | 100.0 | 0.53 | +| noise_burst_recovery | DLL excess | 10.49 | +19.0% | 18.00 | 100.0 | 0.49 | +| noise_burst_recovery | DLL excess+mean | 10.49 | +19.0% | 18.00 | 100.0 | 0.49 | +| noise_burst_recovery | Oracle variance | 10.47 | +19.2% | 17.90 | 100.0 | 0.49 | +| dll_transient_stress | Mevcut C/N0 | 10.15 | +0.0% | 17.43 | 100.0 | 0.36 | +| dll_transient_stress | Naif DLL ekle | 10.12 | +0.3% | 17.33 | 100.0 | 0.38 | +| dll_transient_stress | DLL excess | 10.14 | +0.0% | 17.38 | 100.0 | 0.38 | +| dll_transient_stress | DLL excess+mean | 10.12 | +0.2% | 17.37 | 100.0 | 0.33 | +| dll_transient_stress | Oracle variance | 10.15 | +0.0% | 17.43 | 100.0 | 0.36 | +| stable_multipath_bias | Mevcut C/N0 | 12.55 | +0.0% | 21.00 | 100.0 | 1.67 | +| stable_multipath_bias | Naif DLL ekle | 12.39 | +1.3% | 20.87 | 100.0 | 1.47 | +| stable_multipath_bias | DLL excess | 12.54 | +0.1% | 21.02 | 100.0 | 1.64 | +| stable_multipath_bias | DLL excess+mean | 12.54 | +0.1% | 21.02 | 100.0 | 1.64 | +| stable_multipath_bias | Oracle variance | 12.55 | +0.0% | 21.00 | 100.0 | 1.67 | +| mixed_multipath | Mevcut C/N0 | 14.43 | +0.0% | 25.38 | 100.0 | 5.64 | +| mixed_multipath | Naif DLL ekle | 11.26 | +22.0% | 19.60 | 100.0 | 0.73 | +| mixed_multipath | DLL excess | 11.07 | +23.3% | 19.23 | 100.0 | 0.73 | +| mixed_multipath | DLL excess+mean | 11.07 | +23.3% | 19.23 | 100.0 | 0.73 | +| mixed_multipath | Oracle variance | 11.06 | +23.4% | 19.20 | 100.0 | 0.73 | +| poor_geometry_hidden_jitter | Mevcut C/N0 | 293.82 | +0.0% | 574.96 | 100.0 | 94.31 | +| poor_geometry_hidden_jitter | Naif DLL ekle | 206.13 | +29.8% | 405.15 | 100.0 | 92.42 | +| poor_geometry_hidden_jitter | DLL excess | 196.87 | +33.0% | 390.64 | 100.0 | 91.40 | +| poor_geometry_hidden_jitter | DLL excess+mean | 196.87 | +33.0% | 390.64 | 100.0 | 91.40 | +| poor_geometry_hidden_jitter | Oracle variance | 196.20 | +33.2% | 386.58 | 100.0 | 91.27 | +| cn0_step_recovery | Mevcut C/N0 | 22.59 | +0.0% | 47.95 | 100.0 | 21.62 | +| cn0_step_recovery | Naif DLL ekle | 22.27 | +1.4% | 47.59 | 100.0 | 21.42 | +| cn0_step_recovery | DLL excess | 22.59 | -0.0% | 47.91 | 100.0 | 21.49 | +| cn0_step_recovery | DLL excess+mean | 22.59 | -0.0% | 47.91 | 100.0 | 21.49 | +| cn0_step_recovery | Oracle variance | 22.59 | +0.0% | 47.95 | 100.0 | 21.62 | + +## Modelden çıkan karar özeti + +- C/N0 ile uyumlu nominal DLL jitter'da `dll_excess`, mevcut çözümü değiştirmemelidir. +- Bütün measurement sigmaları aynı sabitle çarpılırsa WLS konum noktası değişmez; yalnız covariance/integrity değişir. Fakat ortak DLL jitter çarpanı, C/N0/elevation bileşimi kanaldan kanala farklı olduğu için toplam sigmaları eşit oranda büyütmez. +- Kanal-bağımlı ve C/N0'da görünmeyen excess jitter varsa dinamik sigma o kanalı down-weight ederek konumu iyileştirebilir. +- Stable multipath bias düşük DLL variance bırakabildiği için variance-only yöntem çözüm değildir. +- 50 Hz'de DLL ratio alpha=0.10 yaklaşık 0.19 s zaman sabiti ve 0.57 s yüzde-95 geçiş verir. +- Aşağıdaki yüzdeler seçilen sentetik hata büyüklüklerine koşulludur; kart için kazanç tahmini değildir. + +## Bozulma penceresi odaklı 3D RMS + +| Senaryo/pencere | Mevcut (m) | DLL excess (m) | DLL excess+mean (m) | Oracle variance (m) | +|---|---:|---:|---:|---:| +| noise_burst_recovery [22,45) s | 17.83 | 11.36 | 11.36 | 11.30 | +| dll_transient_stress [25,34) s | 10.83 | 10.82 | 10.65 | 10.83 | +| cn0_step_recovery [5,30) s | 36.48 | 36.48 | 36.48 | 36.48 | + +## Code smoothing ve carrier CFO + +| Carrier-rate koşulu | Ham RMSE (m) | Kör IIR RMSE (m) | Raw Doppler RMSE (m) | FW-IIR Doppler RMSE (m) | Kör IIR peak (m) | Raw Doppler peak (m) | FW-IIR Doppler peak (m) | +|---|---:|---:|---:|---:|---:|---:|---:| +| cfo_compensated | 3.99 | 248.61 | 0.94 | 16.42 | 414.08 | 2.05 | 84.13 | +| cfo_residual_0p5_mps | 3.99 | 248.61 | 1.30 | 16.41 | 414.08 | 2.57 | 83.23 | +| cfo_residual_3_mps | 3.99 | 248.61 | 5.48 | 17.06 | 414.08 | 7.07 | 78.73 | + +## Yorum sınırları + +- `current_cn0`, firmware'deki URA/elevation/C/N0 sigma modelidir. +- `naive_add_dll`, aynı thermal jitter'ı iki kere saymanın negatif kontrolüdür. +- `dll_excess`, yalnız ölçülen DLL variance'ın C/N0 beklentisini aşan kısmını code variance'a ekler. +- `dll_excess_guard`, variance'a ek olarak sürekli discriminator mean stress'ini kullanır. +- `oracle_variance`, sentetik gerçek jitter çarpanını bilir; yalnız variance kaynakları için erişilebilir üst sınırdır, bias'ı bilmez. +- Stable multipath DLL'nin yeni denge noktasında düşük residual variance bırakabilir. Bu nedenle variance-only yönteminin onu çözmesi beklenmez. +- Discriminator gürültü ölçeği kart correlator kayıtları gelene kadar kalibrasyon varsayımıdır; oran tabanlı yöntem mutlak ölçeğe göre daha az hassastır. +- Sonuçlar bitstream veya firmware değişikliği değildir; karar öncesi executable tasarım keşfidir. diff --git a/soc/software/gps_measurement_python/output/DOPPLER_ESKF_ANALYSIS.md b/soc/software/gps_measurement_python/output/DOPPLER_ESKF_ANALYSIS.md new file mode 100644 index 0000000..55df3c4 --- /dev/null +++ b/soc/software/gps_measurement_python/output/DOPPLER_ESKF_ANALYSIS.md @@ -0,0 +1,65 @@ +# Doppler WLS -> ESKF model sonuçları + +Monte Carlo seed sayısı: `20`; ilk 5 saniye RMS metriklerinden çıkarıldı. +50 Hz tracking, önceki `alpha=0.05` Doppler IIR, 5 Hz dört-state velocity WLS, fusion adapter'ın `2/2/3 m/s` ENU sigma tabanları, TDK profiliyle 100 Hz IMU propagation ve 3-DOF `%99.9` NIS kapısı modellenmiştir. + +## Sonuç tablosu + +| Senaryo | Yol | ESKF RMS (m/s) | Dynamic RMS | WLS RMS | Accept (%) | 95% cov. (%) | Exclude (%) | +|---|---|---:|---:|---:|---:|---:|---:| +| steady_cruise | Önceki sabit IIR | 0.182 | - | 0.068 | 100.0 | 100.0 | 0.0 | +| steady_cruise | Ham Doppler + ESKF | 0.190 | - | 0.416 | 100.0 | 100.0 | 0.0 | +| steady_cruise | Adaptif IIR + ESKF | 0.182 | - | 0.068 | 100.0 | 100.0 | 0.0 | +| steady_cruise | Ham + robust WLS + ESKF | 0.190 | - | 0.416 | 100.0 | 100.0 | 0.0 | +| airliner_maneuver | Önceki sabit IIR | 0.416 | 0.438 | 0.440 | 100.0 | 99.7 | 0.0 | +| airliner_maneuver | Ham Doppler + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 | +| airliner_maneuver | Adaptif IIR + ESKF | 0.194 | 0.189 | 0.135 | 100.0 | 100.0 | 0.0 | +| airliner_maneuver | Ham + robust WLS + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 | +| aggressive_maneuver | Önceki sabit IIR | 0.942 | 0.919 | 1.178 | 100.0 | 55.9 | 0.0 | +| aggressive_maneuver | Ham Doppler + ESKF | 0.190 | 0.186 | 0.416 | 100.0 | 100.0 | 0.0 | +| aggressive_maneuver | Adaptif IIR + ESKF | 0.200 | 0.197 | 0.171 | 100.0 | 100.0 | 0.0 | +| aggressive_maneuver | Ham + robust WLS + ESKF | 0.190 | 0.186 | 0.416 | 100.0 | 100.0 | 0.0 | +| low_cn0_airliner | Önceki sabit IIR | 0.419 | 0.443 | 0.485 | 100.0 | 99.7 | 0.0 | +| low_cn0_airliner | Ham Doppler + ESKF | 0.251 | 0.242 | 1.317 | 100.0 | 100.0 | 0.0 | +| low_cn0_airliner | Adaptif IIR + ESKF | 0.206 | 0.203 | 0.338 | 100.0 | 100.0 | 0.0 | +| low_cn0_airliner | Ham + robust WLS + ESKF | 0.251 | 0.242 | 1.317 | 100.0 | 100.0 | 0.0 | +| single_satellite_outliers | Önceki sabit IIR | 0.495 | 0.478 | 1.146 | 100.0 | 97.7 | 0.0 | +| single_satellite_outliers | Ham Doppler + ESKF | 0.257 | 0.229 | 1.451 | 100.0 | 100.0 | 0.0 | +| single_satellite_outliers | Adaptif IIR + ESKF | 0.356 | 0.312 | 1.154 | 100.0 | 100.0 | 0.0 | +| single_satellite_outliers | Ham + robust WLS + ESKF | 0.191 | 0.179 | 0.418 | 100.0 | 100.0 | 2.9 | +| clock_drift_steps | Önceki sabit IIR | 0.416 | 0.438 | 0.440 | 100.0 | 99.7 | 0.0 | +| clock_drift_steps | Ham Doppler + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 | +| clock_drift_steps | Adaptif IIR + ESKF | 0.194 | 0.189 | 0.135 | 100.0 | 100.0 | 0.0 | +| clock_drift_steps | Ham + robust WLS + ESKF | 0.190 | 0.179 | 0.416 | 100.0 | 100.0 | 0.0 | +| gps_gap_recovery | Önceki sabit IIR | 0.397 | 0.413 | 0.431 | 100.0 | 99.9 | 0.0 | +| gps_gap_recovery | Ham Doppler + ESKF | 0.208 | 0.192 | 0.417 | 100.0 | 100.0 | 0.0 | +| gps_gap_recovery | Adaptif IIR + ESKF | 0.208 | 0.197 | 0.129 | 100.0 | 100.0 | 0.0 | +| gps_gap_recovery | Ham + robust WLS + ESKF | 0.208 | 0.192 | 0.417 | 100.0 | 100.0 | 0.0 | +| mixed_flight_faults | Önceki sabit IIR | 0.940 | 0.913 | 1.515 | 100.0 | 60.4 | 0.0 | +| mixed_flight_faults | Ham Doppler + ESKF | 0.224 | 0.209 | 1.498 | 100.0 | 100.0 | 0.0 | +| mixed_flight_faults | Adaptif IIR + ESKF | 0.295 | 0.275 | 0.838 | 100.0 | 99.9 | 0.0 | +| mixed_flight_faults | Ham + robust WLS + ESKF | 0.221 | 0.206 | 1.013 | 100.0 | 100.0 | 1.9 | + +## Bulgular + +- Sabit hızda pre-IIR ESKF RMS'i `0.182` m/s, ham yol `0.190` m/s. Yani pre-IIR sakin durumda daha düzgün çıktı veriyor; ESKF ham gürültüyü zaten `0.416` m/s WLS RMS'ten bu seviyeye indiriyor. +- Airliner manevrasında sabit IIR `0.416` m/s ve `%0.0` NIS ret oranı verirken ham yol `0.190` m/s ve `%0.0` ret verdi. Konservatif adapter tabanları rejection'ı sınırlasa bile covariance'ın temsil etmediği IIR gecikmesi fused velocity'de dinamik hata bırakır. +- Düşük C/N0 manevrasında sabit/ham/adaptif RMS sırasıyla `0.419`, `0.251`, `0.206` m/s oldu. Adaptif yol bu sentetik koşulda gürültü/gecikme uzlaşmasını sağladı; gerçek eşikler kart verisi ister. +- Tek uydu burst'ünde raw WLS RMS `1.451` m/s, robust leave-one-out ile `0.418` m/s. ESKF son RMS farkı daha küçük; çünkü mevcut residual covariance inflation aggregate hatayı zaten zayıflatıyor. Robust kapı yine de hatalı kanalı bütün epoch'u reddetmeden ayırır. +- Common clock-drift step raw velocity RMS'ini bozmadı; clock-drift state RMS'i `0.269` m/s. Dördüncü WLS bilinmeyeni ortak frekans hatasını üç hız bileşeninden ayırıyor. + +## 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. +- 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. + +## Model sınırları + +- Sonuçlar sentetik hata büyüklüklerine koşulludur; kartta yüzde kazanç iddiası değildir. +- Uydu geometrisi sabit sekiz-kanal local LOS modelidir. Satellite velocity/clock düzeltmelerinin doğru yapıldığı varsayılmıştır. +- ESKF, bu soruya etki eden `[velocity, accel-bias]` alt sistemine indirgenmiştir; attitude, lever-arm ve tam position cross-covariance modellenmemiştir. +- Tam firmware'deki position update'in velocity/accel-bias cross-covariance üzerinden sağlayabileceği kısmi recovery bu azaltılmış modelde yoktur. Bu nedenle uzun NIS-ret zincirinin mutlak büyüklüğü değil, pre-IIR'in gecikme ve ret mekanizması karar girdisidir. +- Carrier loop gürültüsü C/N0 sigma yasasıyla üretilmiştir. Gerçek correlation, oscillator phase noise ve cycle slip dağılımları kart loguyla kalibre edilmelidir. +- Robust WLS'nin `3.5 sigma`, en az altı uydu ve bütün tek-kanal leave-one-out seçimi C implementasyonuyla aynıdır. RTL değişmez; raw/filtered ayrımı firmware'dedir. diff --git a/soc/software/gps_measurement_python/output/doppler_eskf_summary.csv b/soc/software/gps_measurement_python/output/doppler_eskf_summary.csv new file mode 100644 index 0000000..a9025cd --- /dev/null +++ b/soc/software/gps_measurement_python/output/doppler_eskf_summary.csv @@ -0,0 +1,33 @@ +scenario,path,seeds,fused_rms_mps,fused_rms_seed_std_mps,fused_p95_mps,quiet_rms_mps,dynamic_rms_mps,wls_rms_mps,wls_p95_mps,clock_drift_rms_mps,velocity_accept_percent,nis_reject_percent,fused_95_coverage_percent,excluded_epoch_percent,mean_covariance_scale,mean_alpha +steady_cruise,fixed_iir_wls_eskf,20,0.1821998934975134,0.040897665127266884,0.27466022199861373,0.1821998934975134,nan,0.0681533332956358,0.11645920599554568,0.04445747832784426,100.0,0.0,100.0,0.0,1.0000030764799477,0.05000000000000001 +steady_cruise,raw_wls_eskf,20,0.19046432375253602,0.040009781644375395,0.28906917942032884,0.19046432375253602,nan,0.4164164729808367,0.720889200812689,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343088,1.0 +steady_cruise,adaptive_iir_wls_eskf,20,0.1821998934975134,0.040897665127266884,0.27466022199861373,0.1821998934975134,nan,0.0681533332956358,0.11645920599554568,0.04445747832784426,100.0,0.0,100.0,0.0,1.0000030764799477,0.05000000000000001 +steady_cruise,raw_robust_wls_eskf,20,0.19046432375253602,0.040009781644375395,0.28906917942032884,0.19046432375253602,nan,0.4164164729808367,0.720889200812689,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343088,1.0 +airliner_maneuver,fixed_iir_wls_eskf,20,0.41605194656716626,0.04357555329949249,0.7083141902703941,0.4019739347287984,0.4380205569605863,0.440222331186762,0.9391237648850084,0.04445747832784397,100.0,0.0,99.71411764705883,0.0,1.0000030764799477,0.05000000000000001 +airliner_maneuver,raw_wls_eskf,20,0.19046432375253933,0.04000978164437598,0.28906917942032373,0.196483054968526,0.1789145728432288,0.4164164729808368,0.720889200812707,0.2689447776657465,100.0,0.0,100.0,0.0,1.2633800541343085,1.0 +airliner_maneuver,adaptive_iir_wls_eskf,20,0.19421091770965,0.04161706926344872,0.2913238026993562,0.19649429208827177,0.18911500227901482,0.1349904626304289,0.2634780317604405,0.06996150326971154,100.0,0.0,100.0,0.0,1.0000030764799477,0.11647550111358576 +airliner_maneuver,raw_robust_wls_eskf,20,0.19046432375253933,0.04000978164437598,0.28906917942032373,0.196483054968526,0.1789145728432288,0.4164164729808368,0.720889200812707,0.2689447776657465,100.0,0.0,100.0,0.0,1.2633800541343085,1.0 +aggressive_maneuver,fixed_iir_wls_eskf,20,0.9420862291590975,0.03697606175587584,1.7661046896298587,0.9551353937753919,0.918769792345089,1.1782538620062029,2.634559496211176,0.04445747832784312,100.0,0.0,55.891176470588235,0.0,1.0000030764799477,0.05000000000000001 +aggressive_maneuver,raw_wls_eskf,20,0.19046432375254996,0.04000978164436178,0.2890691794203225,0.1928303776618579,0.18561935253806933,0.41641647298083734,0.7208892008127159,0.2689447776657467,100.0,0.0,100.0,0.0,1.2633800541343083,1.0 +aggressive_maneuver,adaptive_iir_wls_eskf,20,0.20034438129208682,0.04215736847319882,0.3027038479248761,0.2017392380134829,0.19713256311771804,0.17059135831478262,0.3555978539018994,0.08938088837068828,100.0,0.0,100.0,0.0,1.0001185178364316,0.17603563474387524 +aggressive_maneuver,raw_robust_wls_eskf,20,0.19046432375254996,0.04000978164436178,0.2890691794203225,0.1928303776618579,0.18561935253806933,0.41641647298083734,0.7208892008127159,0.2689447776657467,100.0,0.0,100.0,0.0,1.2633800541343083,1.0 +low_cn0_airliner,fixed_iir_wls_eskf,20,0.4194988293998554,0.04110688289349549,0.7094271250760412,0.4045729059455276,0.4427791039662977,0.4845544319763686,0.9431345662991619,0.14058689054355467,100.0,0.0,99.74176470588235,0.0,1.0000281042403611,0.05000000000000001 +low_cn0_airliner,raw_wls_eskf,20,0.25103588282812916,0.04464089418791806,0.4041748199164089,0.2555504221567221,0.2416319579439005,1.3168245098334108,2.2796518151866563,0.8504780622313423,100.0,0.0,100.0,0.0,1.26385941740462,1.0 +low_cn0_airliner,adaptive_iir_wls_eskf,20,0.20588009483954214,0.040099541607423995,0.3090368268997948,0.2069944088941925,0.20293127741446826,0.337603473203961,0.684354700050294,0.21325106769335284,100.0,0.0,100.0,0.0,1.0000902389090656,0.1040055679287305 +low_cn0_airliner,raw_robust_wls_eskf,20,0.25103588282812916,0.04464089418791806,0.4041748199164089,0.2555504221567221,0.2416319579439005,1.3168245098334108,2.2796518151866563,0.8504780622313423,100.0,0.0,100.0,0.0,1.26385941740462,1.0 +single_satellite_outliers,fixed_iir_wls_eskf,20,0.49511238386612133,0.053875439119808514,0.8173709856169348,0.5050054814659805,0.4779542859711989,1.1463378050509494,0.9892652696081077,0.8027251360930692,100.0,0.0,97.6964705882353,0.0,6.927634385063575,0.05000000000000001 +single_satellite_outliers,raw_wls_eskf,20,0.2570227443883875,0.05273042903908908,0.39439133440568985,0.27216019157834215,0.22927677845507954,1.4512624341492657,0.8297158565959334,1.0630956381619745,100.0,0.0,100.0,0.0,11.13597527632786,1.0 +single_satellite_outliers,adaptive_iir_wls_eskf,20,0.3556263234893831,0.05851649747663683,0.595609347562141,0.3792437361871613,0.31186327209433895,1.1538474734782178,0.5683617007748845,0.8527337411854743,100.0,0.0,100.0,0.0,7.7254775572955,0.11647550111358576 +single_satellite_outliers,raw_robust_wls_eskf,20,0.19054941715698687,0.03971061631217381,0.2889429468862187,0.19658211712738688,0.1789788721793858,0.4181969381744313,0.7254268588122093,0.27034697594947305,100.0,0.0,100.0,2.8953229398663693,1.2641999517115465,1.0 +clock_drift_steps,fixed_iir_wls_eskf,20,0.4160519465671664,0.04357555329949111,0.7083141902703948,0.4019739347287993,0.4380205569605854,0.44022233118676146,0.9391237648850096,1.8177258394243663,100.0,0.0,99.71411764705883,0.0,1.0000030764799477,0.05000000000000001 +clock_drift_steps,raw_wls_eskf,20,0.19046432375253802,0.04000978164437644,0.28906917942032334,0.19648305496852497,0.17891457284322693,0.41641647298083695,0.7208892008126868,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343085,1.0 +clock_drift_steps,adaptive_iir_wls_eskf,20,0.19421091770965143,0.0416170692634497,0.29132380269935765,0.19649429208827301,0.18911500227901648,0.1349904626304289,0.26347803176043993,1.818528619384018,100.0,0.0,100.0,0.0,1.0000030764799477,0.11647550111358576 +clock_drift_steps,raw_robust_wls_eskf,20,0.19046432375253802,0.04000978164437644,0.28906917942032334,0.19648305496852497,0.17891457284322693,0.41641647298083695,0.7208892008126868,0.268944777665747,100.0,0.0,100.0,0.0,1.2633800541343085,1.0 +gps_gap_recovery,fixed_iir_wls_eskf,20,0.3971781427894736,0.044796626065109466,0.7104470646835127,0.3873196668983798,0.41253405912166263,0.43094056066637465,0.9463346669492851,0.04458606585950929,100.0,0.0,99.90529411764706,0.0,1.0000036934211136,0.05 +gps_gap_recovery,raw_wls_eskf,20,0.2082177514862905,0.03682283889189333,0.32879462613500177,0.21668463381045563,0.1921901394048915,0.41707211010710354,0.7203717161799947,0.26913433965877315,100.0,0.0,100.0,0.0,1.2631217416869522,1.0 +gps_gap_recovery,adaptive_iir_wls_eskf,20,0.20776527476484566,0.044556420313135676,0.3257106582800945,0.21357805324970008,0.19661285828638408,0.12935782738837362,0.2613749696102362,0.06764578524374312,100.0,0.0,100.0,0.0,1.0000036934211136,0.10814839572192514 +gps_gap_recovery,raw_robust_wls_eskf,20,0.2082177514862905,0.03682283889189333,0.32879462613500177,0.21668463381045563,0.1921901394048915,0.41707211010710354,0.7203717161799947,0.26913433965877315,100.0,0.0,100.0,0.0,1.2631217416869522,1.0 +mixed_flight_faults,fixed_iir_wls_eskf,20,0.9404176641021843,0.04319034103529422,1.7661662430364344,0.9555417869207146,0.9132850762857121,1.5154618461064036,2.663221132275041,0.7591896512045151,100.0,0.0,60.37294117647059,0.0,1.3397749809447188,0.05000000000000001 +mixed_flight_faults,raw_wls_eskf,20,0.22399771088975795,0.05069473595820174,0.3492819306443341,0.23151007332767243,0.2092729705094658,1.498455933396739,2.36689254151483,0.7603191788358898,100.0,0.0,100.0,0.0,2.1864248911994575,1.0 +mixed_flight_faults,adaptive_iir_wls_eskf,20,0.2952659489493427,0.07200078881833771,0.4874378367724045,0.30541797568187434,0.2754557634719251,0.838415542523151,0.9361678311586857,0.47500979673114047,100.0,0.0,99.92470588235294,0.0,1.250042525522558,0.178938679245283 +mixed_flight_faults,raw_robust_wls_eskf,20,0.2208132635451996,0.04372541489397352,0.3416439647301807,0.22864253849532457,0.20560052604298312,1.0129266912203836,2.2537697549366906,0.673490930924677,100.0,0.0,100.0,1.9103773584905661,1.5692458289027678,1.0 diff --git a/soc/software/gps_measurement_python/output/scenario_summary.csv b/soc/software/gps_measurement_python/output/scenario_summary.csv new file mode 100644 index 0000000..5c1ed52 --- /dev/null +++ b/soc/software/gps_measurement_python/output/scenario_summary.csv @@ -0,0 +1,51 @@ +scenario,method,epochs,horizontal_rms_m,position_3d_rms_m,position_3d_p95_m,valid_percent,valid_3d_rms_m,horizontal_2sigma_coverage_percent,false_valid_over_25m_percent,fde_percent,mean_measurement_sigma_m +clean_open_sky,current_cn0,4500,5.778805688412197,10.054230284262685,17.35250490247279,100.0,10.054230284262685,99.97777777777777,0.3111111111111111,0.0,8.840973580918838 +clean_open_sky,naive_add_dll,4500,5.768593681325036,10.022431815549082,17.253420427433362,100.0,10.022431815549082,100.0,0.3111111111111111,0.0,10.452052333958017 +clean_open_sky,dll_excess,4500,5.7784464702133524,10.05299246387535,17.320782018324902,100.0,10.05299246387535,99.97777777777777,0.33333333333333337,0.0,8.908174172560404 +clean_open_sky,dll_excess_guard,4500,5.7784464702133524,10.05299246387535,17.320782018324902,100.0,10.05299246387535,99.97777777777777,0.33333333333333337,0.0,8.908174172560404 +clean_open_sky,oracle_variance,4500,5.778805688412197,10.054230284262685,17.35250490247279,100.0,10.054230284262685,99.97777777777777,0.3111111111111111,0.0,8.840973580918838 +weak_signal,current_cn0,4500,19.020289863331797,32.73415409569256,56.44388754013162,100.0,32.73415409569256,98.8,56.31111111111111,0.6222222222222222,20.84621216112656 +weak_signal,naive_add_dll,4500,18.802930957724488,32.313115706990146,55.45511304556428,100.0,32.313115706990146,100.0,56.177777777777784,0.0,28.674069502747358 +weak_signal,dll_excess,4500,18.986226727568837,32.6486650471064,56.115244341908046,100.0,32.6486650471064,98.91111111111111,56.222222222222214,0.5111111111111112,21.20058629982459 +weak_signal,dll_excess_guard,4500,18.986226727568837,32.6486650471064,56.115244341908046,100.0,32.6486650471064,98.91111111111111,56.222222222222214,0.5111111111111112,21.20058629982459 +weak_signal,oracle_variance,4500,19.020289863331797,32.73415409569256,56.44388754013162,100.0,32.73415409569256,98.8,56.31111111111111,0.6222222222222222,20.84621216112656 +one_channel_hidden_jitter,current_cn0,4500,8.804153682379612,12.268224803643907,19.936371518380813,100.0,12.268224803643907,96.46666666666667,1.0666666666666667,9.666666666666666,8.840973580918838 +one_channel_hidden_jitter,naive_add_dll,4500,6.529225405554112,10.487953223937716,17.74604536639535,100.0,10.487953223937716,99.97777777777777,0.33333333333333337,0.0,11.805515833602682 +one_channel_hidden_jitter,dll_excess,4500,6.455903980559815,10.46584556632944,17.675128387138177,100.0,10.46584556632944,99.97777777777777,0.33333333333333337,0.044444444444444446,10.358668146326426 +one_channel_hidden_jitter,dll_excess_guard,4500,6.455903980559815,10.46584556632944,17.675128387138177,100.0,10.46584556632944,99.97777777777777,0.33333333333333337,0.044444444444444446,10.358668146326426 +one_channel_hidden_jitter,oracle_variance,4500,6.4491935598492525,10.463101509886709,17.736218582311878,100.0,10.463101509886709,99.97777777777777,0.33333333333333337,0.06666666666666667,10.315229331071063 +common_hidden_jitter,current_cn0,4500,19.733300930595462,33.79270185129192,59.76307034516212,94.75555555555556,33.15386252421736,56.39999999999999,52.400000000000006,46.577777777777776,8.840973580918838 +common_hidden_jitter,naive_add_dll,4500,16.033969673897577,27.61407554068951,47.55915451805768,100.0,27.61407554068951,99.46666666666667,43.93333333333334,0.26666666666666666,18.882004393424943 +common_hidden_jitter,dll_excess,4500,16.085615037536463,27.688964211504615,47.828684727805175,100.0,27.688964211504615,98.97777777777777,43.888888888888886,0.4444444444444444,18.04555007574287 +common_hidden_jitter,dll_excess_guard,4500,16.085615037536463,27.688964211504615,47.828684727805175,100.0,27.688964211504615,98.97777777777777,43.888888888888886,0.4444444444444444,18.04555007574287 +common_hidden_jitter,oracle_variance,4500,16.106964558267112,27.740128015702773,47.83188793884836,100.0,27.740128015702773,98.88888888888889,43.86666666666667,0.4888888888888889,18.035214487051437 +noise_burst_recovery,current_cn0,4500,9.325915933961204,12.96069950118485,22.132869786417263,98.93333333333332,12.552632065384056,94.71111111111111,2.577777777777778,12.11111111111111,8.840973580918838 +noise_burst_recovery,naive_add_dll,4500,6.470839383968352,10.523606281690133,18.031622500344657,100.0,10.523606281690133,100.0,0.5333333333333333,0.06666666666666667,11.60221083527078 +noise_burst_recovery,dll_excess,4500,6.397124977878571,10.493909835494788,17.99630946143617,100.0,10.493909835494788,99.97777777777777,0.4888888888888889,0.13333333333333333,10.131003114059961 +noise_burst_recovery,dll_excess_guard,4500,6.397124977878571,10.493909835494788,17.99630946143617,100.0,10.493909835494788,99.97777777777777,0.4888888888888889,0.13333333333333333,10.131003114059961 +noise_burst_recovery,oracle_variance,4500,6.367173301653832,10.473649717876942,17.90212603773505,100.0,10.473649717876942,99.97777777777777,0.4888888888888889,0.044444444444444446,10.187907003764948 +dll_transient_stress,current_cn0,4500,5.803776392869777,10.145785618162538,17.425227799952054,100.0,10.145785618162538,99.97777777777777,0.35555555555555557,0.0,8.840973580918838 +dll_transient_stress,naive_add_dll,4500,5.793995520088368,10.118871049708806,17.333372587049784,100.0,10.118871049708806,100.0,0.37777777777777777,0.0,10.45205253980181 +dll_transient_stress,dll_excess,4500,5.8034283726190745,10.144627133073621,17.380953993070463,100.0,10.144627133073621,99.97777777777777,0.37777777777777777,0.0,8.908174353046816 +dll_transient_stress,dll_excess_guard,4500,5.79072382051268,10.123295335548953,17.368844982312577,100.0,10.123295335548953,99.97777777777777,0.33333333333333337,0.0,8.947656011912292 +dll_transient_stress,oracle_variance,4500,5.803776392869777,10.145785618162538,17.425227799952054,100.0,10.145785618162538,99.97777777777777,0.35555555555555557,0.0,8.840973580918838 +stable_multipath_bias,current_cn0,4500,7.12694286036133,12.545413964352555,20.99675149186582,100.0,12.545413964352555,99.8888888888889,1.6666666666666667,0.022222222222222223,8.840973580918838 +stable_multipath_bias,naive_add_dll,4500,7.072680893174009,12.386663925234807,20.866871605887997,100.0,12.386663925234807,99.97777777777777,1.4666666666666666,0.0,10.452052333958017 +stable_multipath_bias,dll_excess,4500,7.125500558670432,12.536689588412326,21.02122766562977,100.0,12.536689588412326,99.91111111111111,1.6444444444444446,0.022222222222222223,8.908174172560404 +stable_multipath_bias,dll_excess_guard,4500,7.125500558670432,12.536689588412326,21.02122766562977,100.0,12.536689588412326,99.91111111111111,1.6444444444444446,0.022222222222222223,8.908174172560404 +stable_multipath_bias,oracle_variance,4500,7.12694286036133,12.545413964352555,20.99675149186582,100.0,12.545413964352555,99.8888888888889,1.6666666666666667,0.022222222222222223,8.840973580918838 +mixed_multipath,current_cn0,4500,8.529840854093363,14.426936007413373,25.381172069686816,100.0,14.426936007413373,97.11111111111111,5.644444444444444,5.0,8.840973580918838 +mixed_multipath,naive_add_dll,4500,6.586489015624914,11.258484405711224,19.599950438767102,100.0,11.258484405711224,100.0,0.7333333333333333,0.044444444444444446,11.636563744509052 +mixed_multipath,dll_excess,4500,6.45479117311332,11.067801578489938,19.233225446062768,100.0,11.067801578489938,100.0,0.7333333333333333,0.08888888888888889,10.202310406784667 +mixed_multipath,dll_excess_guard,4500,6.45479117311332,11.067801578489938,19.233225446062768,100.0,11.067801578489938,100.0,0.7333333333333333,0.08888888888888889,10.202310406784667 +mixed_multipath,oracle_variance,4500,6.448333589783235,11.058021985103961,19.19754215965981,100.0,11.058021985103961,99.95555555555555,0.7333333333333333,0.08888888888888889,10.144307488606447 +poor_geometry_hidden_jitter,current_cn0,4500,79.38795978543642,293.8222926191477,574.9617330272,100.0,293.8222926191477,79.0,94.31111111111112,1.1555555555555554,10.734703317378012 +poor_geometry_hidden_jitter,naive_add_dll,4500,57.59864532145799,206.13456070122297,405.1527013193,100.0,206.13456070122297,99.66666666666667,92.42222222222222,0.022222222222222223,14.604104871190987 +poor_geometry_hidden_jitter,dll_excess,4500,55.3927819274298,196.86935630824075,390.63687676393124,100.0,196.86935630824075,99.24444444444444,91.4,0.022222222222222223,12.779670971493758 +poor_geometry_hidden_jitter,dll_excess_guard,4500,55.3927819274298,196.86935630824075,390.63687676393124,100.0,196.86935630824075,99.24444444444444,91.4,0.022222222222222223,12.779670971493758 +poor_geometry_hidden_jitter,oracle_variance,4500,55.24662489772763,196.2046647890204,386.5771391089706,100.0,196.2046647890204,99.28888888888889,91.26666666666667,0.022222222222222223,12.721163297137666 +cn0_step_recovery,current_cn0,4500,12.974359636882712,22.585010269106,47.95054105970103,100.0,22.585010269106,99.42222222222222,21.622222222222224,0.2222222222222222,13.475047639913695 +cn0_step_recovery,naive_add_dll,4500,12.797089787478507,22.265941453345402,47.59129168788298,100.0,22.265941453345402,100.0,21.422222222222224,0.0,17.44366868773946 +cn0_step_recovery,dll_excess,4500,12.96657717120691,22.586113735655264,47.91159461508304,100.0,22.586113735655264,99.46666666666667,21.488888888888887,0.2,13.653801199442478 +cn0_step_recovery,dll_excess_guard,4500,12.96657717120691,22.586113735655264,47.91159461508304,100.0,22.586113735655264,99.46666666666667,21.488888888888887,0.2,13.653801199442478 +cn0_step_recovery,oracle_variance,4500,12.974359636882712,22.585010269106,47.95054105970103,100.0,22.585010269106,99.42222222222222,21.622222222222224,0.2222222222222222,13.475047639913695 diff --git a/soc/software/gps_measurement_python/requirements.txt b/soc/software/gps_measurement_python/requirements.txt new file mode 100644 index 0000000..e7ea6c1 --- /dev/null +++ b/soc/software/gps_measurement_python/requirements.txt @@ -0,0 +1,2 @@ +numpy>=1.24 +matplotlib>=3.7 diff --git a/soc/software/gps_measurement_python/rtl_dll_model.py b/soc/software/gps_measurement_python/rtl_dll_model.py new file mode 100644 index 0000000..7298e9f --- /dev/null +++ b/soc/software/gps_measurement_python/rtl_dll_model.py @@ -0,0 +1,200 @@ +"""Small executable model of the RTL code-tracking observables. + +This is deliberately scoped to the parts that matter to the navigation +measurement study: + +* the normalized early-minus-late magnitude discriminator; +* its signed Q2.13 output; +* the fixed C1/C2 code-loop filter and Q20 NCO command; +* the 20 one-millisecond discriminator samples available per NAV bit. + +It is not an RF waveform simulator. The C/N0-to-discriminator-noise mapping +is an explicit calibration assumption until real correlator/DLL telemetry is +recorded from the board. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +DLL_OUTPUT_FRACTION_BITS = 13 +DLL_OUTPUT_WORD_BITS = 15 +DLL_SPACING_CHIPS = 0.5 +CODE_LOOP_C1 = 21.25 +CODE_LOOP_C2 = 0.05712890625 +CODE_LOOP_OUTPUT_FRACTION_BITS = 20 +CODE_LOOP_OUTPUT_WORD_BITS = 28 +DLL_SAMPLES_PER_NAV_BIT = 20 +CODE_FREQUENCY_BASIS_HZ = 1.023e6 +ABS_SAMPLE_COUNTER_MODULUS = 1 << 32 + +# Synthetic calibration point. It only fixes the scale of the Monte Carlo +# discriminator telemetry. Dynamic weighting uses a measured/expected ratio, +# so the absolute choice cancels in the correctly modelled case. +DLL_STD_AT_45_DBHZ = 0.020 + + +def _signed_saturate(value: int, word_bits: int) -> int: + minimum = -(1 << (word_bits - 1)) + maximum = (1 << (word_bits - 1)) - 1 + return min(maximum, max(minimum, value)) + + +def quantize_signed(value: float, word_bits: int, fraction_bits: int) -> int: + scaled = int(np.rint(value * (1 << fraction_bits))) + return _signed_saturate(scaled, word_bits) + + +def dequantize_signed(value: int, fraction_bits: int) -> float: + return float(value) / float(1 << fraction_bits) + + +def triangular_correlation(code_offset_chips: float) -> float: + """Ideal one-chip triangular GPS C/A autocorrelation main lobe.""" + + return max(0.0, 1.0 - abs(code_offset_chips)) + + +def ideal_early_late_magnitudes( + code_error_chips: float, + spacing_chips: float = DLL_SPACING_CHIPS, +) -> tuple[float, float]: + half_spacing = 0.5 * spacing_chips + early = triangular_correlation(code_error_chips + half_spacing) + late = triangular_correlation(code_error_chips - half_spacing) + return early, late + + +def normalized_early_minus_late(early_magnitude: float, late_magnitude: float) -> float: + denominator = early_magnitude + late_magnitude + if denominator <= 0.0: + return 0.0 + return (early_magnitude - late_magnitude) / denominator + + +def rtl_discriminator(code_error_chips: float) -> tuple[int, float]: + """Return the RTL-like Q2.13 discriminator code and dequantized value.""" + + early, late = ideal_early_late_magnitudes(code_error_chips) + value = normalized_early_minus_late(early, late) + code = quantize_signed( + value, + DLL_OUTPUT_WORD_BITS, + DLL_OUTPUT_FRACTION_BITS, + ) + return code, dequantize_signed(code, DLL_OUTPUT_FRACTION_BITS) + + +def expected_discriminator_std(cn0_dbhz: float) -> float: + """Expected normalized DLL jitter used by the synthetic calibration. + + The 20 dB amplitude-domain law mirrors the firmware's code-sigma model. + The result is bounded because the normalized discriminator is finite. + """ + + value = DLL_STD_AT_45_DBHZ * 10.0 ** ((45.0 - cn0_dbhz) / 20.0) + return float(np.clip(value, 0.002, 0.35)) + + +def quantized_code_epoch_measurement( + true_abs_sample: float, +) -> tuple[int, int, float]: + """Model ABS_SAMPLE + REM_CODE_PHASE and firmware reconstruction. + + ``true_abs_sample`` is the ideal code boundary in RF-sample units. The + hardware captures the following integer sample and reports the overshoot + with the remainder scale used by the C correction formula. + """ + + raw_unwrapped = int(np.ceil(true_abs_sample)) + overshoot_samples = raw_unwrapped - true_abs_sample + remainder = int(np.rint(overshoot_samples * CODE_FREQUENCY_BASIS_HZ)) + raw_register = raw_unwrapped % ABS_SAMPLE_COUNTER_MODULUS + corrected = raw_unwrapped - remainder / CODE_FREQUENCY_BASIS_HZ + return raw_register, remainder, corrected + + +@dataclass(frozen=True) +class DllWindow: + samples: np.ndarray + mean: float + variance: float + expected_variance: float + variance_ratio: float + mean_z_score: float + + +def synthesize_dll_window( + rng: np.random.Generator, + *, + cn0_dbhz: float, + jitter_multiplier: float = 1.0, + discriminator_bias: float = 0.0, + sample_count: int = DLL_SAMPLES_PER_NAV_BIT, +) -> DllWindow: + """Generate and Q2.13-quantize one NAV-bit DLL telemetry window.""" + + if sample_count < 2: + raise ValueError("DLL variance needs at least two samples") + expected_std = expected_discriminator_std(cn0_dbhz) + actual_std = expected_std * max(0.0, jitter_multiplier) + raw = discriminator_bias + rng.normal(0.0, actual_std, sample_count) + minimum = -(1 << (DLL_OUTPUT_WORD_BITS - 1)) + maximum = (1 << (DLL_OUTPUT_WORD_BITS - 1)) - 1 + quantized_codes = np.clip( + np.rint(raw * (1 << DLL_OUTPUT_FRACTION_BITS)), + minimum, + maximum, + ).astype(np.int32) + samples = quantized_codes.astype(float) / float(1 << DLL_OUTPUT_FRACTION_BITS) + mean = float(np.mean(samples)) + variance = float(np.var(samples, ddof=1)) + expected_variance = expected_std * expected_std + ratio = variance / max(expected_variance, 1.0e-15) + standard_error = np.sqrt(max(variance, expected_variance) / sample_count) + mean_z = abs(mean) / max(float(standard_error), 1.0e-12) + return DllWindow( + samples=samples, + mean=mean, + variance=variance, + expected_variance=expected_variance, + variance_ratio=float(ratio), + mean_z_score=float(mean_z), + ) + + +@dataclass +class CodeLoopFilterRtl: + """Update-rate model of LF_code.sv, including coefficient quantization.""" + + integrator_q20: int = 0 + + c1_integer: int = int(CODE_LOOP_C1 * (1 << 2)) + c2_integer: int = int(CODE_LOOP_C2 * (1 << 11)) + + def update_code(self, discriminator_q13: int) -> int: + # C1 product has Q15 and is zero-padded to Q20 in LF_code.sv. + proportional_q20 = (self.c1_integer * discriminator_q13) << 5 + + # C2 product has Q24 and LF_code.sv slices it back to Q20. + integral_increment_q20 = (self.c2_integer * discriminator_q13) >> 4 + self.integrator_q20 = _signed_saturate( + self.integrator_q20 + integral_increment_q20, + CODE_LOOP_OUTPUT_WORD_BITS, + ) + return _signed_saturate( + self.integrator_q20 + proportional_q20, + CODE_LOOP_OUTPUT_WORD_BITS, + ) + + def update(self, discriminator: float) -> float: + discriminator_q13 = quantize_signed( + discriminator, + DLL_OUTPUT_WORD_BITS, + DLL_OUTPUT_FRACTION_BITS, + ) + command_q20 = self.update_code(discriminator_q13) + return dequantize_signed(command_q20, CODE_LOOP_OUTPUT_FRACTION_BITS) diff --git a/soc/software/gps_measurement_python/test_doppler_eskf_model.py b/soc/software/gps_measurement_python/test_doppler_eskf_model.py new file mode 100644 index 0000000..bf678ee --- /dev/null +++ b/soc/software/gps_measurement_python/test_doppler_eskf_model.py @@ -0,0 +1,148 @@ +"""Regression tests for the project-specific Doppler/WLS/ESKF model.""" + +from __future__ import annotations + +import unittest + +import numpy as np + +from .doppler_eskf_model import ( + AdaptiveAlpha, + DopplerPath, + FIXED_DOPPLER_ALPHA, + flight_scenarios, + local_los_matrix, + run_doppler_eskf_scenario, + velocity_wls, +) + + +def _scenario(name: str): + return next(item for item in flight_scenarios() if item.name == name) + + +def _fused_rms(trace) -> float: + selected = trace.time_s >= 5.0 + error = trace.fused_velocity_mps[selected] - trace.truth_velocity_mps[selected] + return float(np.sqrt(np.mean(np.sum(np.square(error), axis=1)))) + + +def _wls_rms(trace) -> float: + truth = np.column_stack( + [ + np.interp( + trace.wls_time_s, + trace.time_s, + trace.truth_velocity_mps[:, axis], + ) + for axis in range(3) + ] + ) + error = trace.wls_velocity_mps - truth + return float(np.sqrt(np.mean(np.sum(np.square(error), axis=1)))) + + +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]) + clock_drift = 18.0 + observation = -local_los_matrix() @ velocity + clock_drift + result = velocity_wls(observation, np.full(8, 45.0)) + + np.testing.assert_allclose(result.velocity_mps, velocity, atol=1.0e-10) + self.assertAlmostEqual(result.clock_drift_mps, clock_drift, places=10) + self.assertAlmostEqual(result.covariance_scale, 1.0) + + def test_robust_wls_excludes_a_single_bad_channel(self) -> None: + velocity = np.asarray([120.0, 20.0, -2.0]) + observation = -local_los_matrix() @ velocity + 4.0 + observation[2] += 12.0 + + plain = velocity_wls(observation, np.full(8, 43.0)) + robust = velocity_wls(observation, np.full(8, 43.0), robust=True) + + self.assertEqual(robust.excluded_satellite, 2) + self.assertGreater(np.linalg.norm(plain.velocity_mps - velocity), 1.0) + np.testing.assert_allclose(robust.velocity_mps, velocity, atol=1.0e-9) + + +class AdaptiveAlphaTest(unittest.TestCase): + def test_motion_switches_alpha_and_quiet_state_recovers(self) -> None: + selector = AdaptiveAlpha() + for _ in range(80): + quiet = selector.update(np.zeros(3), 35.0) + self.assertAlmostEqual(quiet, 0.03) + + dynamic_values = [ + selector.update(np.asarray([4.0, 0.0, 0.0]), 35.0) + for _ in range(30) + ] + self.assertGreaterEqual(max(dynamic_values), 0.40) + + for _ in range(120): + recovered = selector.update(np.zeros(3), 40.0) + self.assertAlmostEqual(recovered, FIXED_DOPPLER_ALPHA) + + +class DopplerEskfIntegrationTest(unittest.TestCase): + def test_eskf_filters_raw_doppler_without_pre_iir(self) -> None: + traces = run_doppler_eskf_scenario( + _scenario("steady_cruise"), + seed=7, + paths=(DopplerPath.RAW,), + ) + raw = traces[DopplerPath.RAW] + self.assertLess(_fused_rms(raw), 0.45 * _wls_rms(raw)) + self.assertGreater(float(np.mean(raw.wls_accepted)), 0.99) + + def test_raw_path_avoids_fixed_iir_manoeuvre_lag(self) -> None: + traces = run_doppler_eskf_scenario( + _scenario("airliner_maneuver"), + seed=3, + paths=(DopplerPath.FIXED_IIR, DopplerPath.RAW), + ) + fixed = traces[DopplerPath.FIXED_IIR] + raw = traces[DopplerPath.RAW] + self.assertLess(_fused_rms(raw), 0.35 * _fused_rms(fixed)) + # The production 2/2/3 m/s adapter floors keep both paths inside the + # NIS gate; the fixed-IIR penalty now appears as lag error, not reject. + self.assertGreater(float(np.mean(raw.wls_accepted)), 0.99) + self.assertGreater(float(np.mean(fixed.wls_accepted)), 0.99) + + def test_common_clock_step_does_not_leak_into_raw_velocity(self) -> None: + airliner = run_doppler_eskf_scenario( + _scenario("airliner_maneuver"), + seed=11, + paths=(DopplerPath.RAW,), + )[DopplerPath.RAW] + clock_step = run_doppler_eskf_scenario( + _scenario("clock_drift_steps"), + seed=11, + paths=(DopplerPath.RAW,), + )[DopplerPath.RAW] + + np.testing.assert_allclose( + airliner.wls_velocity_mps, + clock_step.wls_velocity_mps, + atol=1.0e-10, + ) + np.testing.assert_allclose( + airliner.fused_velocity_mps, + clock_step.fused_velocity_mps, + atol=1.0e-10, + ) + + def test_robust_wls_contains_channel_bursts_before_eskf(self) -> None: + traces = run_doppler_eskf_scenario( + _scenario("single_satellite_outliers"), + seed=5, + paths=(DopplerPath.RAW, DopplerPath.RAW_ROBUST), + ) + raw = traces[DopplerPath.RAW] + robust = traces[DopplerPath.RAW_ROBUST] + self.assertLess(_wls_rms(robust), 0.65 * _wls_rms(raw)) + self.assertGreater(float(np.mean(robust.excluded_satellites >= 0)), 0.02) + + +if __name__ == "__main__": + unittest.main() diff --git a/soc/software/gps_measurement_python/test_measurement_model.py b/soc/software/gps_measurement_python/test_measurement_model.py new file mode 100644 index 0000000..bd0fc7d --- /dev/null +++ b/soc/software/gps_measurement_python/test_measurement_model.py @@ -0,0 +1,182 @@ +from __future__ import annotations + +import unittest + +import numpy as np + +from gps_measurement_python.code_smoothing_model import run_smoothing_trial +from gps_measurement_python.firmware_wls_model import ( + SigmaInputs, + SigmaMethod, + firmware_sigma_m, +) +from gps_measurement_python.measurement_scenarios import ( + run_scenario, + scenario_matrix, +) +from gps_measurement_python.rtl_dll_model import ( + CodeLoopFilterRtl, + quantized_code_epoch_measurement, + rtl_discriminator, + synthesize_dll_window, +) + + +class RtlDllModelTests(unittest.TestCase): + def test_discriminator_zero_and_q13_symmetry(self): + self.assertEqual(rtl_discriminator(0.0)[0], 0) + positive = rtl_discriminator(0.05)[1] + negative = rtl_discriminator(-0.05)[1] + self.assertAlmostEqual(positive, -negative, places=6) + self.assertLess(positive, 0.0) + + def test_code_loop_integrator_accumulates_constant_error(self): + loop = CodeLoopFilterRtl() + first = loop.update(0.01) + twentieth = first + for _ in range(19): + twentieth = loop.update(0.01) + self.assertGreater(first, 0.0) + self.assertGreater(twentieth, first) + + def test_dll_window_detects_excess_variance(self): + rng = np.random.default_rng(42) + nominal = synthesize_dll_window( + rng, + cn0_dbhz=45.0, + jitter_multiplier=1.0, + sample_count=4000, + ) + noisy = synthesize_dll_window( + rng, + cn0_dbhz=45.0, + jitter_multiplier=3.0, + sample_count=4000, + ) + self.assertAlmostEqual(nominal.variance_ratio, 1.0, delta=0.10) + self.assertAlmostEqual(noisy.variance_ratio, 9.0, delta=0.70) + + def test_abs_sample_remainder_reconstructs_subsample_epoch(self): + for true_sample in ( + 12_345_678.314159, + 12_345_678.0000001, + 12_345_678.9999999, + 12_345_678.0, + ): + raw, remainder, corrected = quantized_code_epoch_measurement(true_sample) + self.assertEqual(raw, int(np.ceil(true_sample))) + self.assertLessEqual( + abs(corrected - true_sample), + 0.5 / 1.023e6 + 1.0e-12, + ) + self.assertGreaterEqual(remainder, 0) + + +class FirmwareSigmaTests(unittest.TestCase): + @staticmethod + def inputs(ratio: float = 1.0, mean_z: float = 0.0) -> SigmaInputs: + return SigmaInputs( + ura_index=np.asarray([0]), + elevation_deg=np.asarray([90.0]), + cn0_dbhz=np.asarray([45.0]), + dll_variance_ratio=np.asarray([ratio]), + dll_mean_z=np.asarray([mean_z]), + actual_jitter_multiplier=np.asarray([np.sqrt(ratio)]), + ) + + def test_current_matches_c_formula(self): + sigma = firmware_sigma_m(self.inputs(), SigmaMethod.CURRENT)[0] + expected = np.sqrt(2.4**2 + 3.0**2 + 3.0**2) + self.assertAlmostEqual(sigma, expected, places=12) + + def test_excess_method_does_not_double_count_nominal_dll_noise(self): + current = firmware_sigma_m(self.inputs(), SigmaMethod.CURRENT)[0] + excess = firmware_sigma_m(self.inputs(), SigmaMethod.DLL_EXCESS)[0] + naive = firmware_sigma_m(self.inputs(), SigmaMethod.NAIVE_ADD_DLL)[0] + self.assertAlmostEqual(excess, current, places=12) + self.assertGreater(naive, current) + + def test_excess_method_inflates_hidden_jitter(self): + current = firmware_sigma_m(self.inputs(9.0), SigmaMethod.CURRENT)[0] + excess = firmware_sigma_m(self.inputs(9.0), SigmaMethod.DLL_EXCESS)[0] + self.assertGreater(excess, 1.8 * current) + + +class ScenarioTests(unittest.TestCase): + def test_hidden_jitter_dynamic_sigma_improves_seeded_case(self): + scenario = next( + item + for item in scenario_matrix() + if item.name == "one_channel_hidden_jitter" + ) + samples = run_scenario( + scenario, + 17, + (SigmaMethod.CURRENT, SigmaMethod.DLL_EXCESS), + ) + current = np.asarray( + [ + item.position_error_3d_m + for item in samples + if item.method == SigmaMethod.CURRENT and item.time_s >= 5.0 + ] + ) + dynamic = np.asarray( + [ + item.position_error_3d_m + for item in samples + if item.method == SigmaMethod.DLL_EXCESS and item.time_s >= 5.0 + ] + ) + self.assertLess( + np.sqrt(np.mean(dynamic * dynamic)), + 0.90 * np.sqrt(np.mean(current * current)), + ) + + def test_stable_multipath_is_not_solved_by_variance_only(self): + scenario = next( + item + for item in scenario_matrix() + if item.name == "stable_multipath_bias" + ) + samples = run_scenario( + scenario, + 23, + (SigmaMethod.CURRENT, SigmaMethod.DLL_EXCESS), + ) + current = np.asarray( + [ + item.position_error_3d_m + for item in samples + if item.method == SigmaMethod.CURRENT and item.time_s >= 5.0 + ] + ) + dynamic = np.asarray( + [ + item.position_error_3d_m + for item in samples + if item.method == SigmaMethod.DLL_EXCESS and item.time_s >= 5.0 + ] + ) + relative_difference = abs( + np.sqrt(np.mean(dynamic * dynamic)) + / np.sqrt(np.mean(current * current)) + - 1.0 + ) + self.assertLess(relative_difference, 0.08) + + def test_doppler_prediction_avoids_blind_iir_dynamic_lag(self): + result = run_smoothing_trial(31, doppler_cfo_mps=0.0) + self.assertLess(result.doppler_smoothed_rmse_m, result.raw_rmse_m) + self.assertLess( + result.doppler_peak_dynamic_error_m, + 0.10 * result.scalar_iir_peak_dynamic_error_m, + ) + self.assertGreater( + result.firmware_iir_doppler_peak_dynamic_error_m, + result.doppler_peak_dynamic_error_m, + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/soc/software/gps_system/README.md b/soc/software/gps_system/README.md index 5175f69..3cb7614 100644 --- a/soc/software/gps_system/README.md +++ b/soc/software/gps_system/README.md @@ -811,23 +811,23 @@ 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. Bu nedenle Doppler ayni PRN icin software -ISR state'inde: +receiver velocity hatasina donusur. Software ISR her PRN icin ham degeri +degistirmeden korur ve yalniz PPS predictor yolu icin ayri bir state'te: ```text doppler_filtered = 0.05 * doppler_raw + 0.95 * doppler_previous ``` -ile filtrelenir ve range-rate filtrelenmis Doppler'dan tekrar hesaplanir. -50 Hz measurement cadence'inde bu yaklasik `0.39 s` zaman sabitidir. -Reacquisition/PRN degisiminde eski kanal state'i kullanilmaz. +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. IIR donanimda da uygulanabilirdi; bunun icin kanal basina state, coefficient-multiply, rounding/saturation ve reacquisition reset kurali gerekir. Mevcut sistemde gerekli degildir: RTL'nin gorevi ham, atomik ve -20 ms sabit olcumu sunmaktir; C/N0/WLS zaten software'dedir, katsayi hareket -profiline gore bitstream almadan degistirilebilir ve ham Doppler debug icin -korunur. Board testinde CPU/interrupt yolu measurement kaybetmedigi icin +20 ms sabit olcumu sunmaktir; C/N0/WLS zaten software'dedir, ham ve filtreli +yollar bitstream almadan ayrilabilir. Board testinde CPU/interrupt yolu +measurement kaybetmedigi icin RTL filtresinin karsilayacagi bir bant genisligi veya latency problemi de gorulmemistir. Ileride tamamen RTL icinde bir tuketici eklenirse ham register yaninda ayri bir filtered register mantikli olabilir. @@ -844,20 +844,71 @@ sigma_rate = clamp(30 * 10^(-C/N0_dbhz / 20), 0.05, 5.0) 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 +``` + +Ham ESKF yolu temporal filtreleme isini ESKF'ye birakir ve `alpha=0.05` +IIR'in yaklasik 390 ms dinamik gecikmesini velocity measurement'a eklemez. +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 +WLS'nin dorduncu `receiver_clock_drift_mps` state'i PPS'e dogrudan verilmez. + +Position WLS ile Doppler WLS iki ayri fiziksel observation kullanir: +code-phase pseudorange konum/ofseti, carrier Doppler ise hiz/frekans hatasini +olcer. Klasik mutlak-pseudorange modelinde ayni receiver zaman hatasi +`delta_t_r(t)` icin: + +```text +code WLS: [x, y, z, b_r], b_r = c * delta_t_r [m] +Doppler WLS: [vx, vy, vz, bdot_r], bdot_r = c * d(delta_t_r)/dt [m/s] +``` + +Doppler WLS'nin dorduncu bilinmeyeni receiver clock/frequency drift'idir. +Receiver oscillator `0.1 ppm` hizliysa `bdot_r` yaklasik `29.98 m/s` olur. +Bu etki butun uydu Doppler'larina ortak girer; cozulmezse bir kismi +`vx/vy/vz` receiver hareketi sanilir. + +Bu projedeki position WLS'e verilen capture zamanlarindan ortak receiver +zamani cikarilir ve `STARTOFFSET` eklenir. Dolayisiyla +`solution.clock_bias_m`, saf `c*delta_t_r` yerine yaklasik +`c*STARTOFFSET - reference_range` olan referans-uyduya bagli goreli bir +datumdur. Doppler `solution.clock_drift_mps` ile bunun arasinda turev kisiti +kurulmaz. + +Fit'ler matematiksel olarak ayridir ve `solution_valid`/`velocity_valid` +bagimsizdir. Bununla birlikte velocity WLS LOS birim vektorlerini hesaplamak +icin position WLS'nin guncel receiver ECEF konumunu kullanir; uydu ECEF +konumlari, hizlari ve satellite clock drift'leri broadcast ephemeristen gelir. +Position kabul edilip velocity reddedilebilir; velocity NIS reject ayni +epoch'taki position update'ini geri almaz. + En az dort uydu gerekir. Dort uyduda velocity residual serbestlik derecesi yoktur; bes uydu ve dusuk PDOP'ta bile zayif geometri velocity gurultusunu -buyutebilir. WLS, `inverse(H'WH)` matrisinden ECEF velocity covariance uretir. -`N>4` ise weighted post-fit reduced chi-square yalniz covariance'i buyutebilir; -tek bir sessiz epoch formal belirsizligi kucultemez. Sonuc `velocity_valid`, -ECEF velocity/covariance, `speed_mps` ve `velocity_residual_rms_mps` -alanlarinda yayinlanir. +buyutebilir. Alti veya daha fazla kullanilabilir uyduda full-fit maksimum +normalized residual `3.5 sigma`'yi asarsa butun tek-kanal leave-one-out +adaylari cozulur; en dusuk weighted post-fit residual enerjili aday secilir. +Boylece ESKF'nin aggregate 3D velocity NIS kapisina gelmeden once bozuk kanal +ayrilabilir. Dort/bes uyduda redundancy yetersiz oldugu icin velocity FDE +uygulanmaz. + +WLS, `inverse(H'WH)` matrisinden ECEF velocity covariance uretir. `N>4` ise +weighted post-fit reduced chi-square yalniz covariance'i buyutebilir; tek bir +sessiz epoch formal belirsizligi kucultemez. Sonuc `velocity_valid`, ECEF +velocity/covariance, `speed_mps`, `velocity_residual_rms_mps`, +`velocity_fde_applied` ve `velocity_excluded_row_mask` alanlarinda yayinlanir. 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. Bunlar IIR ile korelasyonlu ard arda -epoch'larin ESKF'yi asiri guvenle cekmesini onler; formal covariance daha -buyukse aynen korunur. Velocity innovation 3-DOF `%99.9` +`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 +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` olarak yayinlanir. @@ -1137,9 +1188,10 @@ make -C soc/software/gps_system/tests sanitize make -C hardware/external/gps/rtl/trck_src/tb test ``` -`run` ve `sanitize`, C/N0 estimator'i, Q7 Doppler/range-rate donusumu, -coherent mailbox read'i, weighted velocity WLS/covariance ve satellite -velocity/clock drift hesaplarini kapsar. Fusion regression'i ayrica +`run` ve `sanitize`, C/N0 estimator'i, Q7 ham/filtreli Doppler-range-rate +donusumu, coherent mailbox read'i, weighted velocity WLS/covariance, +tek-kanal robust FDE ve satellite velocity/clock drift hesaplarini kapsar. +Fusion regression'i ayrica ECEF->ENU velocity/covariance rotasyonunu, velocity NIS kabul/redini ve ayni epoch position+velocity delayed replay esdegerligini kapsar. RTL Questa/vlog-vsim testi prompt akumulasyonu, @@ -1260,8 +1312,10 @@ T_rx = TOW_ref + POSTPONE_TOW + STARTOFFSET/1000 - b/c diye `1.024 us` tutulur. Watchdog, quality holdover ve `20 ppm` slew clamp buyuk arizalari ayri olarak sinirlar. - Yazilim filtresi PI'nin yerine gecmez. Yalnizca 5 Hz WLS clock-bias - olcumunde Doppler clock-drift prediction, bounded innovation ve aykiri - olcum reddi yapar. Ilk yerlesmede commit'ler coarse ve PPS invalid tutulur. + olcumunde filtreli-Doppler velocity'den turetilen referans-datum drift + prediction, bounded innovation ve aykiri olcum reddi yapar. Position WLS + `clock_bias_m` ana PPS olcumu olarak kalir; Doppler yalnız prediction'dir. + Ilk yerlesmede commit'ler coarse ve PPS invalid tutulur. Periyodik background watchdog iki saniye yeni referans gelmezse cikis validligini holdover ile kapatir; fakat ayni referans geri gelirse Doppler prediction ve donanim PI state'i 30 saniyeye kadar korunur. Referans SVID diff --git a/soc/software/gps_system/header/gps/NavEquations.h b/soc/software/gps_system/header/gps/NavEquations.h index aec4b49..e979e35 100644 --- a/soc/software/gps_system/header/gps/NavEquations.h +++ b/soc/software/gps_system/header/gps/NavEquations.h @@ -32,8 +32,12 @@ double elevation_deg[GPS_TRCK_IP_COUNT]; double measurement_sigma_m[GPS_TRCK_IP_COUNT]; double cn0_dbhz[GPS_TRCK_IP_COUNT]; + /* Raw Doppler is published with the ESKF velocity solution. */ double doppler_hz[GPS_TRCK_IP_COUNT]; double pseudorange_rate_mps[GPS_TRCK_IP_COUNT]; + /* Separate slow path used only by the PPS bias-drift predictor. */ + double filtered_doppler_hz[GPS_TRCK_IP_COUNT]; + double filtered_pseudorange_rate_mps[GPS_TRCK_IP_COUNT]; double full_postfit_residual_m[GPS_TRCK_IP_COUNT]; double full_elevation_deg[GPS_TRCK_IP_COUNT]; double full_measurement_sigma_m[GPS_TRCK_IP_COUNT]; @@ -53,6 +57,7 @@ unsigned int degrees_of_freedom; uint32_t used_row_mask; uint32_t excluded_row_mask; + uint32_t velocity_excluded_row_mask; uint64_t candidate_cycles; uint64_t full_solution_cycles; uint64_t fde_cycles; @@ -65,6 +70,7 @@ bool fde_applied; bool solution_valid; bool velocity_valid; + bool velocity_fde_applied; } gps_nav_solution_t; void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr); diff --git a/soc/software/gps_system/header/gps/gps_tracking_measurement.h b/soc/software/gps_system/header/gps/gps_tracking_measurement.h index 50143d3..2be86c8 100644 --- a/soc/software/gps_system/header/gps/gps_tracking_measurement.h +++ b/soc/software/gps_system/header/gps/gps_tracking_measurement.h @@ -17,8 +17,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. */ double doppler_hz; double pseudorange_rate_mps; + /* Slow software IIR path retained only for PPS bias-drift prediction. */ + double filtered_doppler_hz; + double filtered_pseudorange_rate_mps; double cn0_dbhz; bool nav_bit; bool locked; @@ -59,4 +63,23 @@ bool gps_tracking_velocity_wls( double* receiver_clock_drift_mps_out, double* residual_rms_mps_out); +/* + * Raw-Doppler velocity solution with a one-channel leave-one-out FDE trial. + * FDE runs only for six or more usable channels and only when the full-fit + * maximum normalized residual exceeds 3.5 sigma. The returned bit mask uses + * indices from the input measurement array, not compacted WLS row indices. + */ +bool gps_tracking_velocity_wls_robust( + const double receiver_ecef_m[3], + const double satellite_positions_m[][3], + const double satellite_velocities_mps[][3], + const double satellite_clock_drifts_sps[], + const gps_tracking_measurement_t measurements[], + unsigned int count, + double receiver_velocity_ecef_mps_out[3], + double receiver_velocity_covariance_ecef_mps2_out[3][3], + double* receiver_clock_drift_mps_out, + double* residual_rms_mps_out, + uint32_t* excluded_measurement_mask_out); + #endif diff --git a/soc/software/gps_system/src/adapters/gps_system_uart.c b/soc/software/gps_system/src/adapters/gps_system_uart.c index 923d1b0..cc09bfb 100644 --- a/soc/software/gps_system/src/adapters/gps_system_uart.c +++ b/soc/software/gps_system/src/adapters/gps_system_uart.c @@ -27,12 +27,13 @@ void gps_system_uart_publish_location( GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION, "source=%s lat=%.20lf lon=%.20lf height=%.3lf " "gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf " - "tracked=%u used=%u velocity_valid=%u speed=%.3lf " + "tracked=%u used=%u velocity_valid=%u velocity_fde=%u speed=%.3lf " "velocity_rms=%.3lf\r\n", source, latitude_deg, longitude_deg, height_m, gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop, gps->tracked_satellite_count, gps->used_satellite_count, gps->velocity_valid ? 1u : 0u, + gps->velocity_fde_applied ? 1u : 0u, gps->speed_mps, gps->velocity_residual_rms_mps); } diff --git a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c index bc0277c..660196c 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -156,13 +156,13 @@ static void gps_update_tracking_measurement_isr( * A 0.05 pole at the 50 Hz navigation-bit cadence has an approximately * 0.4 s time constant while retaining real receiver dynamics. */ - measurement.doppler_hz = + measurement.filtered_doppler_hz = GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz + (1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) * - previous->doppler_hz; - measurement.pseudorange_rate_mps = + previous->filtered_doppler_hz; + measurement.filtered_pseudorange_rate_mps = gps_tracking_doppler_to_range_rate_mps( - measurement.doppler_hz); + measurement.filtered_doppler_hz); } measurement.local_timestamp_ticks = local_timestamp_ticks; gps_latest_tracking_measurement[tracking_index] = measurement; diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index 72f0f64..8cd0285 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -853,6 +853,9 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, bool ok = false; bool pps_clock_bias_drift_valid = false; double pps_clock_bias_drift_mps = 0.0; + bool pps_velocity_valid = false; + double pps_velocity_residual_rms_mps = 0.0; + uint32_t pps_velocity_excluded_row_mask = 0u; gps_nav_solution_t solution; memset(&solution, 0, sizeof(solution)); memset(solution_out, 0, sizeof(*solution_out)); @@ -954,6 +957,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, solution.doppler_hz[row] = measurements[row].doppler_hz; solution.pseudorange_rate_mps[row] = measurements[row].pseudorange_rate_mps; + solution.filtered_doppler_hz[row] = + measurements[row].filtered_doppler_hz; + solution.filtered_pseudorange_rate_mps[row] = + measurements[row].filtered_pseudorange_rate_mps; } if(ok && solution.solution_valid) { @@ -976,7 +983,7 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, velocity_measurements[row].valid = false; } } - solution.velocity_valid = gps_tracking_velocity_wls( + solution.velocity_valid = gps_tracking_velocity_wls_robust( solution.ecef_m, satellite_positions_array, satellite_velocities_array, @@ -986,7 +993,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, solution.velocity_ecef_mps, solution.velocity_covariance_ecef_mps2, &solution.clock_drift_mps, - &solution.velocity_residual_rms_mps); + &solution.velocity_residual_rms_mps, + &solution.velocity_excluded_row_mask); + solution.velocity_fde_applied = + solution.velocity_excluded_row_mask != 0u; if(solution.velocity_valid) { solution.speed_mps = @@ -996,15 +1006,57 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, solution.velocity_ecef_mps[1] + solution.velocity_ecef_mps[2] * solution.velocity_ecef_mps[2]); + } + + /* + * 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. + */ + for(unsigned int row = 0u; row < satellite_count; row++) + { + velocity_measurements[row].doppler_hz = + measurements[row].filtered_doppler_hz; + velocity_measurements[row].pseudorange_rate_mps = + measurements[row].filtered_pseudorange_rate_mps; + } + double pps_velocity_ecef_mps[3]; + double pps_velocity_covariance_ecef_mps2[3][3]; + double pps_receiver_clock_drift_mps = 0.0; + pps_velocity_valid = gps_tracking_velocity_wls_robust( + solution.ecef_m, + satellite_positions_array, + satellite_velocities_array, + clockDrift, + velocity_measurements, + satellite_count, + pps_velocity_ecef_mps, + pps_velocity_covariance_ecef_mps2, + &pps_receiver_clock_drift_mps, + &pps_velocity_residual_rms_mps, + &pps_velocity_excluded_row_mask); + if(pps_velocity_valid) + { pps_clock_bias_drift_valid = gps_pps_reference_bias_drift( solution.ecef_m, - solution.velocity_ecef_mps, + pps_velocity_ecef_mps, satellite_positions_array[reference_index], satellite_velocities_array[reference_index], clockDrift[reference_index], &pps_clock_bias_drift_mps); } + if(solution.velocity_fde_applied || + (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", + (unsigned long)solution.velocity_excluded_row_mask, + (unsigned long)pps_velocity_excluded_row_mask, + solution.velocity_residual_rms_mps, + pps_velocity_residual_rms_mps); + } } if(ok && solution.fde_applied) { @@ -1056,12 +1108,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, (cn0_count != 0u) ? minimum_cn0 : NAN; pps_quality.chi_square_ratio = solution.chi_square_ratio; pps_quality.velocity_residual_rms_mps = - solution.velocity_valid ? - solution.velocity_residual_rms_mps : 0.0; + pps_velocity_valid ? pps_velocity_residual_rms_mps : 0.0; pps_quality.used_satellites = solution.used_satellite_count; pps_quality.residual_test_valid = solution.residual_test_valid; - pps_quality.velocity_valid = solution.velocity_valid; - pps_quality.fde_applied = solution.fde_applied; + pps_quality.velocity_valid = pps_velocity_valid; + pps_quality.fde_applied = + solution.fde_applied || + (pps_velocity_excluded_row_mask != 0u); } bool pps_profile_changed = false; /* @@ -1141,7 +1194,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, "drift=%.3lf tdop=%.2lf\r\n", gps_pps_profile_name(pps_profile), solution.clock_bias_m, - solution.clock_drift_mps, + pps_clock_bias_drift_valid ? + pps_clock_bias_drift_mps : NAN, solution.tdop); goto pps_anchor_done; } diff --git a/soc/software/gps_system/src/gps/gps_tracking_measurement.c b/soc/software/gps_system/src/gps/gps_tracking_measurement.c index 16ed743..a997bb7 100644 --- a/soc/software/gps_system/src/gps/gps_tracking_measurement.c +++ b/soc/software/gps_system/src/gps/gps_tracking_measurement.c @@ -8,6 +8,8 @@ #define GPS_TRACK_MEASUREMENT_MIN_SATELLITES 4u #define GPS_TRACK_MEASUREMENT_MAX_SATELLITES GPS_TRCK_IP_COUNT #define GPS_TRACK_MEASUREMENT_EPSILON 1.0e-12 +#define GPS_TRACK_VELOCITY_FDE_MIN_SATELLITES 6u +#define GPS_TRACK_VELOCITY_FDE_SIGMA_GATE 3.5 static double clamp_double(double value, double minimum, double maximum) { @@ -135,6 +137,10 @@ bool gps_tracking_measurement_read( measurement_out->pseudorange_rate_mps = gps_tracking_doppler_to_range_rate_mps( measurement_out->doppler_hz); + measurement_out->filtered_doppler_hz = + measurement_out->doppler_hz; + measurement_out->filtered_pseudorange_rate_mps = + measurement_out->pseudorange_rate_mps; unsigned int version = meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT; @@ -149,7 +155,9 @@ bool gps_tracking_measurement_read( (measurement_out->cn0_sample_count == 20u) && cn0_valid && isfinite(measurement_out->doppler_hz) && - isfinite(measurement_out->pseudorange_rate_mps); + isfinite(measurement_out->pseudorange_rate_mps) && + isfinite(measurement_out->filtered_doppler_hz) && + isfinite(measurement_out->filtered_pseudorange_rate_mps); return measurement_out->valid; } @@ -261,7 +269,94 @@ static bool invert_four_by_four(double input[4][4], double inverse[4][4]) return true; } -bool gps_tracking_velocity_wls( +typedef struct { + double solution[4]; + double normal_inverse[4][4]; + double residual_mps[GPS_TRACK_MEASUREMENT_MAX_SATELLITES]; + double residual_square_sum; + double weighted_residual_square_sum; + unsigned int row_count; +} gps_velocity_wls_candidate_t; + +static bool gps_velocity_solve_candidate( + const double design[][4], + const double observation[], + const double weights[], + unsigned int used, + unsigned int excluded_row, + gps_velocity_wls_candidate_t* candidate_out) +{ + if((design == NULL) || (observation == NULL) || (weights == NULL) || + (candidate_out == NULL) || + (used < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) || + (used > GPS_TRACK_MEASUREMENT_MAX_SATELLITES)) + { + return false; + } + + double normal[4][4] = {{0.0}}; + double rhs[4] = {0.0}; + unsigned int selected = 0u; + for(unsigned int measurement_row = 0u; + measurement_row < used; + measurement_row++) + { + if(measurement_row == excluded_row) + { + continue; + } + double weight = weights[measurement_row]; + for(unsigned int row = 0u; row < 4u; row++) + { + rhs[row] += weight * design[measurement_row][row] * + observation[measurement_row]; + for(unsigned int column = 0u; column < 4u; column++) + { + normal[row][column] += + weight * design[measurement_row][row] * + design[measurement_row][column]; + } + } + selected++; + } + if(selected < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) + { + return false; + } + + memset(candidate_out, 0, sizeof(*candidate_out)); + if(!solve_four_by_four(normal, rhs, candidate_out->solution) || + !invert_four_by_four(normal, candidate_out->normal_inverse)) + { + return false; + } + for(unsigned int measurement_row = 0u; + measurement_row < used; + measurement_row++) + { + candidate_out->residual_mps[measurement_row] = NAN; + if(measurement_row == excluded_row) + { + continue; + } + double predicted = 0.0; + for(unsigned int column = 0u; column < 4u; column++) + { + predicted += design[measurement_row][column] * + candidate_out->solution[column]; + } + double residual = observation[measurement_row] - predicted; + candidate_out->residual_mps[measurement_row] = residual; + candidate_out->residual_square_sum += residual * residual; + candidate_out->weighted_residual_square_sum += + weights[measurement_row] * residual * residual; + } + candidate_out->row_count = selected; + return isfinite(candidate_out->residual_square_sum) && + isfinite(candidate_out->weighted_residual_square_sum); +} + +static bool gps_tracking_velocity_wls_internal( const double receiver_ecef_m[3], const double satellite_positions_m[][3], const double satellite_velocities_mps[][3], @@ -271,8 +366,14 @@ bool gps_tracking_velocity_wls( double receiver_velocity_ecef_mps_out[3], double receiver_velocity_covariance_ecef_mps2_out[3][3], double* receiver_clock_drift_mps_out, - double* residual_rms_mps_out) + double* residual_rms_mps_out, + bool robust, + uint32_t* excluded_measurement_mask_out) { + if(excluded_measurement_mask_out != NULL) + { + *excluded_measurement_mask_out = 0u; + } if((receiver_ecef_m == NULL) || (satellite_positions_m == NULL) || (satellite_velocities_mps == NULL) || (satellite_clock_drifts_sps == NULL) || @@ -287,11 +388,10 @@ bool gps_tracking_velocity_wls( return false; } - double normal[4][4] = {{0.0}}; - double rhs[4] = {0.0}; double design[GPS_TRACK_MEASUREMENT_MAX_SATELLITES][4]; double observation[GPS_TRACK_MEASUREMENT_MAX_SATELLITES]; double weights[GPS_TRACK_MEASUREMENT_MAX_SATELLITES]; + unsigned int measurement_index[GPS_TRACK_MEASUREMENT_MAX_SATELLITES]; unsigned int used = 0u; for(unsigned int satellite = 0u; satellite < count; satellite++) @@ -342,17 +442,7 @@ bool gps_tracking_velocity_wls( sigma_rate_mps = clamp_double(sigma_rate_mps, 0.05, 5.0); double weight = 1.0 / (sigma_rate_mps * sigma_rate_mps); weights[used] = weight; - for(unsigned int row = 0u; row < 4u; row++) - { - rhs[row] += - weight * design[used][row] * observation[used]; - for(unsigned int column = 0u; column < 4u; column++) - { - normal[row][column] += - weight * design[used][row] * - design[used][column]; - } - } + measurement_index[used] = satellite; used++; } @@ -361,27 +451,47 @@ bool gps_tracking_velocity_wls( return false; } - double solution[4]; - double normal_inverse[4][4]; - if(!solve_four_by_four(normal, rhs, solution) || - !invert_four_by_four(normal, normal_inverse)) + gps_velocity_wls_candidate_t full; + if(!gps_velocity_solve_candidate( + design, observation, weights, used, + GPS_TRACK_MEASUREMENT_MAX_SATELLITES, &full)) { return false; } - double residual_square_sum = 0.0; - double weighted_residual_square_sum = 0.0; - for(unsigned int row = 0u; row < used; row++) + gps_velocity_wls_candidate_t selected = full; + unsigned int excluded_row = GPS_TRACK_MEASUREMENT_MAX_SATELLITES; + if(robust && (used >= GPS_TRACK_VELOCITY_FDE_MIN_SATELLITES)) { - double predicted = 0.0; - for(unsigned int column = 0u; column < 4u; column++) + double maximum_normalized_residual = 0.0; + for(unsigned int row = 0u; row < used; row++) { - predicted += design[row][column] * solution[column]; + double normalized = + fabs(full.residual_mps[row]) * sqrt(weights[row]); + maximum_normalized_residual = + fmax(maximum_normalized_residual, normalized); } - double residual = observation[row] - predicted; - residual_square_sum += residual * residual; - weighted_residual_square_sum += - weights[row] * residual * residual; + if(maximum_normalized_residual > GPS_TRACK_VELOCITY_FDE_SIGMA_GATE) + { + double best_score = INFINITY; + for(unsigned int trial_row = 0u; trial_row < used; trial_row++) + { + gps_velocity_wls_candidate_t trial; + if(gps_velocity_solve_candidate( + design, observation, weights, used, trial_row, &trial) && + (trial.weighted_residual_square_sum < best_score)) + { + best_score = trial.weighted_residual_square_sum; + selected = trial; + excluded_row = trial_row; + } + } + } + } + if((excluded_row < used) && (excluded_measurement_mask_out != NULL)) + { + *excluded_measurement_mask_out = + 1u << measurement_index[excluded_row]; } /* @@ -392,17 +502,19 @@ bool gps_tracking_velocity_wls( * the poor geometry appear directly in the inverse normal matrix. */ double covariance_scale = 1.0; - if(used > 4u) + if(selected.row_count > 4u) { covariance_scale = fmax( - 1.0, weighted_residual_square_sum / (double)(used - 4u)); + 1.0, + selected.weighted_residual_square_sum / + (double)(selected.row_count - 4u)); } for(unsigned int row = 0u; row < 3u; row++) { for(unsigned int column = 0u; column < 3u; column++) { receiver_velocity_covariance_ecef_mps2_out[row][column] = - covariance_scale * normal_inverse[row][column]; + covariance_scale * selected.normal_inverse[row][column]; if(!isfinite( receiver_velocity_covariance_ecef_mps2_out[row][column])) { @@ -410,10 +522,53 @@ bool gps_tracking_velocity_wls( } } } - memcpy(receiver_velocity_ecef_mps_out, solution, - 3u * sizeof(solution[0])); - *receiver_clock_drift_mps_out = solution[3]; + memcpy(receiver_velocity_ecef_mps_out, selected.solution, + 3u * sizeof(selected.solution[0])); + *receiver_clock_drift_mps_out = selected.solution[3]; *residual_rms_mps_out = - sqrt(residual_square_sum / (double)used); + sqrt(selected.residual_square_sum / (double)selected.row_count); return isfinite(*residual_rms_mps_out); } + +bool gps_tracking_velocity_wls( + const double receiver_ecef_m[3], + const double satellite_positions_m[][3], + const double satellite_velocities_mps[][3], + const double satellite_clock_drifts_sps[], + const gps_tracking_measurement_t measurements[], + unsigned int count, + double receiver_velocity_ecef_mps_out[3], + double receiver_velocity_covariance_ecef_mps2_out[3][3], + double* receiver_clock_drift_mps_out, + double* residual_rms_mps_out) +{ + return gps_tracking_velocity_wls_internal( + receiver_ecef_m, satellite_positions_m, + satellite_velocities_mps, satellite_clock_drifts_sps, + measurements, count, receiver_velocity_ecef_mps_out, + receiver_velocity_covariance_ecef_mps2_out, + receiver_clock_drift_mps_out, residual_rms_mps_out, + false, NULL); +} + +bool gps_tracking_velocity_wls_robust( + const double receiver_ecef_m[3], + const double satellite_positions_m[][3], + const double satellite_velocities_mps[][3], + const double satellite_clock_drifts_sps[], + const gps_tracking_measurement_t measurements[], + unsigned int count, + double receiver_velocity_ecef_mps_out[3], + double receiver_velocity_covariance_ecef_mps2_out[3][3], + double* receiver_clock_drift_mps_out, + double* residual_rms_mps_out, + uint32_t* excluded_measurement_mask_out) +{ + return gps_tracking_velocity_wls_internal( + receiver_ecef_m, satellite_positions_m, + satellite_velocities_mps, satellite_clock_drifts_sps, + measurements, count, receiver_velocity_ecef_mps_out, + receiver_velocity_covariance_ecef_mps2_out, + receiver_clock_drift_mps_out, residual_rms_mps_out, + true, excluded_measurement_mask_out); +} diff --git a/soc/software/gps_system/tests/test_gps_tracking_measurement.c b/soc/software/gps_system/tests/test_gps_tracking_measurement.c index 9611f26..aa45c1a 100644 --- a/soc/software/gps_system/tests/test_gps_tracking_measurement.c +++ b/soc/software/gps_system/tests/test_gps_tracking_measurement.c @@ -78,6 +78,14 @@ static void test_cn0_and_mailbox_decode(void) measurement.pseudorange_rate_mps, expected_range_rate, 1.0e-9)); + assert(close_to( + measurement.filtered_doppler_hz, + measurement.doppler_hz, + 1.0e-12)); + assert(close_to( + measurement.filtered_pseudorange_rate_mps, + measurement.pseudorange_rate_mps, + 1.0e-12)); registers.MEASUREMENT_META &= ~(1u << GPS_TRCK_MEAS_META_LOCK_POS); @@ -180,6 +188,86 @@ static void test_velocity_wls(void) solved_velocity_covariance[0][1], solved_velocity_covariance[1][0], 1.0e-12)); + { + double clean_velocity[3]; + double clean_covariance[3][3]; + double clean_clock_drift; + double clean_residual_rms; + uint32_t clean_excluded_mask = UINT32_MAX; + assert(gps_tracking_velocity_wls_robust( + receiver_position, + satellite_positions, + satellite_velocities, + satellite_clock_drift, + measurements, + TEST_SATELLITES, + clean_velocity, + clean_covariance, + &clean_clock_drift, + &clean_residual_rms, + &clean_excluded_mask)); + assert(clean_excluded_mask == 0u); + for(unsigned int component = 0u; component < 3u; component++) + { + assert(close_to( + clean_velocity[component], + solved_velocity[component], + 1.0e-12)); + } + } + + /* One gross carrier burst must be isolated before aggregate ESKF fusion. */ + measurements[2].pseudorange_rate_mps += 12.0; + double plain_velocity[3]; + double plain_covariance[3][3]; + double plain_clock_drift; + double plain_residual_rms; + assert(gps_tracking_velocity_wls( + receiver_position, + satellite_positions, + satellite_velocities, + satellite_clock_drift, + measurements, + TEST_SATELLITES, + plain_velocity, + plain_covariance, + &plain_clock_drift, + &plain_residual_rms)); + double robust_velocity[3]; + double robust_covariance[3][3]; + double robust_clock_drift; + double robust_residual_rms; + uint32_t excluded_mask = 0u; + assert(gps_tracking_velocity_wls_robust( + receiver_position, + satellite_positions, + satellite_velocities, + satellite_clock_drift, + measurements, + TEST_SATELLITES, + robust_velocity, + robust_covariance, + &robust_clock_drift, + &robust_residual_rms, + &excluded_mask)); + assert(excluded_mask == (1u << 2)); + double plain_error_square = 0.0; + for(unsigned int component = 0u; component < 3u; component++) + { + double plain_error = + plain_velocity[component] - expected_velocity[component]; + plain_error_square += plain_error * plain_error; + assert(close_to( + robust_velocity[component], + expected_velocity[component], + 1.0e-8)); + } + assert(sqrt(plain_error_square) > 1.0); + assert(close_to( + robust_clock_drift, + expected_clock_drift_mps, + 1.0e-8)); + assert(robust_residual_rms < 1.0e-8); } int main(void)