Files
gps/soc/software/gps_measurement_python/analyze_doppler_eskf.py
T

429 lines
19 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""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()