feat(gps): split Doppler paths for ESKF and PPS
This commit is contained in:
@@ -0,0 +1,3 @@
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.venv/
|
||||
@@ -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.
|
||||
@@ -0,0 +1 @@
|
||||
"""Project-specific GPS code-measurement and WLS analysis model."""
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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]
|
||||
)
|
||||
)
|
||||
),
|
||||
)
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 86 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 213 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 41 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 84 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 176 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 184 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 352 KiB |
@@ -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.
|
||||
@@ -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.
|
||||
@@ -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
|
||||
|
@@ -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
|
||||
|
@@ -0,0 +1,2 @@
|
||||
numpy>=1.24
|
||||
matplotlib>=3.7
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user