"""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ı", "", "- Nominal sentetik model ham Doppler'in sabit pre-IIR gecikmesini önlediğini gösterir; fakat Genesys2 kart-kalibreli noise regresyonunda tam ham yol fazla gürültülüdür. Production ana yolu: hareket-adaptif Doppler -> bandwidth-aware sigma -> kanal validity/outlier kontrolü -> 5 Hz robust velocity WLS + gerçek covariance -> adapter tabanları -> ESKF.", "- ESKF yalnız 3D aggregate velocity gördüğü için tek uydu hatasını sonradan teşhis edemez. Per-satellite robust gate/FDE WLS öncesinde kalmalıdır.", "- Ayrı sabit `alpha=0.05` yol PPS drift predictor ve standalone sakin-hız kalite metriği için tutulur; ham snapshot tanısal/offline analiz için korunur.", "- Production adaptif IIR sakin/hareketli/yüksek-ivme için `alpha=0.05/0.20/0.40` seçer. Alpha büyüdükçe EWMA noise gain'i WLS sigma'sına yansıtılır; böylece FDE ve ESKF covariance bant genişliğini bilir.", "", "## Model sınırları", "", "- 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()