Add GPS integrity checks and GPS IMU ESKF fusion tools
This commit is contained in:
@@ -0,0 +1,208 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Compare stationary GPS firmware UART logs, grouped by satellite count."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import math
|
||||
import re
|
||||
import statistics
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
POSITION_RE = re.compile(
|
||||
r"position lat=(?P<lat>[-+0-9.eE]+) "
|
||||
r"lon=(?P<lon>[-+0-9.eE]+) height=(?P<height>[-+0-9.eE]+)"
|
||||
)
|
||||
RUNNING_RE = re.compile(r"navigation software running with (?P<sats>\d+) satellites")
|
||||
NAVSOL_RE = re.compile(
|
||||
r"NAVSOL valid=(?P<valid>[01]).*?fde=(?P<fde>[01]).*?"
|
||||
r"sats=(?P<used>\d+)/(?P<available>\d+)"
|
||||
)
|
||||
|
||||
|
||||
def percentile(values: list[float], percentage: float) -> float:
|
||||
if not values:
|
||||
return math.nan
|
||||
ordered = sorted(values)
|
||||
position = (len(ordered) - 1) * percentage / 100.0
|
||||
lower = math.floor(position)
|
||||
upper = math.ceil(position)
|
||||
if lower == upper:
|
||||
return ordered[lower]
|
||||
fraction = position - lower
|
||||
return ordered[lower] * (1.0 - fraction) + ordered[upper] * fraction
|
||||
|
||||
|
||||
def parse_log(path: Path) -> list[dict[str, float | int | None]]:
|
||||
samples: list[dict[str, float | int | None]] = []
|
||||
current_satellites: int | None = None
|
||||
with path.open(newline="", encoding="utf-8", errors="replace") as stream:
|
||||
for row in csv.DictReader(stream):
|
||||
line = row.get("uart_line", "")
|
||||
running = RUNNING_RE.search(line)
|
||||
if running:
|
||||
current_satellites = int(running.group("sats"))
|
||||
position = POSITION_RE.search(line)
|
||||
if position:
|
||||
samples.append(
|
||||
{
|
||||
"time": float(row.get("host_monotonic_s", "nan")),
|
||||
"lat": float(position.group("lat")),
|
||||
"lon": float(position.group("lon")),
|
||||
"height": float(position.group("height")),
|
||||
"satellites": current_satellites,
|
||||
"used": None,
|
||||
"valid": None,
|
||||
"fde": None,
|
||||
}
|
||||
)
|
||||
navsol = NAVSOL_RE.search(line)
|
||||
if navsol and samples:
|
||||
sample = samples[-1]
|
||||
sample["satellites"] = int(navsol.group("available"))
|
||||
sample["used"] = int(navsol.group("used"))
|
||||
sample["valid"] = int(navsol.group("valid"))
|
||||
sample["fde"] = int(navsol.group("fde"))
|
||||
current_satellites = int(navsol.group("available"))
|
||||
return [sample for sample in samples if sample["satellites"] is not None]
|
||||
|
||||
|
||||
def local_offsets(
|
||||
sample: dict[str, float | int | None], reference: tuple[float, float, float]
|
||||
) -> tuple[float, float, float]:
|
||||
latitude, longitude, height = reference
|
||||
latitude_rad = math.radians(latitude)
|
||||
sin_latitude = math.sin(latitude_rad)
|
||||
eccentricity_squared = 6.69437999014e-3
|
||||
semi_major_axis = 6378137.0
|
||||
prime_vertical = semi_major_axis / math.sqrt(
|
||||
1.0 - eccentricity_squared * sin_latitude * sin_latitude
|
||||
)
|
||||
meridional = (
|
||||
semi_major_axis
|
||||
* (1.0 - eccentricity_squared)
|
||||
/ (1.0 - eccentricity_squared * sin_latitude * sin_latitude) ** 1.5
|
||||
)
|
||||
north = math.radians(float(sample["lat"]) - latitude) * (meridional + height)
|
||||
east = (
|
||||
math.radians(float(sample["lon"]) - longitude)
|
||||
* (prime_vertical + height)
|
||||
* math.cos(latitude_rad)
|
||||
)
|
||||
up = float(sample["height"]) - height
|
||||
return east, north, up
|
||||
|
||||
|
||||
def summarize_group(
|
||||
samples: list[dict[str, float | int | None]], reference: tuple[float, float, float]
|
||||
) -> dict[str, float | int]:
|
||||
offsets = [local_offsets(sample, reference) for sample in samples]
|
||||
horizontal = [math.hypot(east, north) for east, north, _ in offsets]
|
||||
distance_3d = [math.sqrt(east * east + north * north + up * up) for east, north, up in offsets]
|
||||
vertical = [abs(up) for _, _, up in offsets]
|
||||
valid_values = [int(sample["valid"]) for sample in samples if sample["valid"] is not None]
|
||||
fde_values = [int(sample["fde"]) for sample in samples if sample["fde"] is not None]
|
||||
return {
|
||||
"samples": len(samples),
|
||||
"horizontal_mean_m": statistics.fmean(horizontal),
|
||||
"horizontal_p50_m": percentile(horizontal, 50.0),
|
||||
"horizontal_p95_m": percentile(horizontal, 95.0),
|
||||
"horizontal_max_m": max(horizontal),
|
||||
"vertical_abs_mean_m": statistics.fmean(vertical),
|
||||
"distance_3d_mean_m": statistics.fmean(distance_3d),
|
||||
"distance_3d_p95_m": percentile(distance_3d, 95.0),
|
||||
"distance_3d_max_m": max(distance_3d),
|
||||
"valid_rate_pct": 100.0 * statistics.fmean(valid_values) if valid_values else math.nan,
|
||||
"fde_rate_pct": 100.0 * statistics.fmean(fde_values) if fde_values else math.nan,
|
||||
}
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--old", type=Path, required=True)
|
||||
parser.add_argument("--new", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
|
||||
logs = {"old": parse_log(args.old), "new": parse_log(args.new)}
|
||||
if not logs["old"] or not logs["new"]:
|
||||
raise SystemExit("both logs must contain position samples with satellite counts")
|
||||
|
||||
references = {
|
||||
name: (
|
||||
statistics.median(float(sample["lat"]) for sample in samples),
|
||||
statistics.median(float(sample["lon"]) for sample in samples),
|
||||
statistics.median(float(sample["height"]) for sample in samples),
|
||||
)
|
||||
for name, samples in logs.items()
|
||||
}
|
||||
|
||||
rows: list[dict[str, str | int | float]] = []
|
||||
for firmware, samples in logs.items():
|
||||
groups: list[tuple[str, list[dict[str, float | int | None]]]] = [("all", samples)]
|
||||
groups.extend(
|
||||
(str(count), [sample for sample in samples if sample["satellites"] == count])
|
||||
for count in sorted({int(sample["satellites"]) for sample in samples})
|
||||
)
|
||||
if any(sample["fde"] is not None for sample in samples):
|
||||
groups.extend(
|
||||
(
|
||||
f"all_fde{fde_state}",
|
||||
[sample for sample in samples if sample["fde"] == fde_state],
|
||||
)
|
||||
for fde_state in (0, 1)
|
||||
if any(sample["fde"] == fde_state for sample in samples)
|
||||
)
|
||||
groups.extend(
|
||||
(
|
||||
f"{count}_fde{fde_state}",
|
||||
[
|
||||
sample
|
||||
for sample in samples
|
||||
if sample["satellites"] == count and sample["fde"] == fde_state
|
||||
],
|
||||
)
|
||||
for count in sorted({int(sample["satellites"]) for sample in samples})
|
||||
for fde_state in (0, 1)
|
||||
if any(
|
||||
sample["satellites"] == count and sample["fde"] == fde_state
|
||||
for sample in samples
|
||||
)
|
||||
)
|
||||
for satellite_count, group in groups:
|
||||
rows.append(
|
||||
{
|
||||
"firmware": firmware,
|
||||
"satellites": satellite_count,
|
||||
**summarize_group(group, references[firmware]),
|
||||
}
|
||||
)
|
||||
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
with args.output.open("w", newline="", encoding="utf-8") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=list(rows[0]))
|
||||
writer.writeheader()
|
||||
writer.writerows(rows)
|
||||
|
||||
old_reference = {"lat": references["old"][0], "lon": references["old"][1], "height": references["old"][2]}
|
||||
new_offset = local_offsets(old_reference, references["new"])
|
||||
median_separation = math.sqrt(sum(value * value for value in new_offset))
|
||||
print(f"old_samples={len(logs['old'])} new_samples={len(logs['new'])}")
|
||||
print(f"old_reference={references['old']}")
|
||||
print(f"new_reference={references['new']}")
|
||||
print(f"median_position_separation_3d_m={median_separation:.3f}")
|
||||
for row in rows:
|
||||
print(
|
||||
f"{row['firmware']:>3} sats={row['satellites']:>3} n={row['samples']:>4} "
|
||||
f"hmean={row['horizontal_mean_m']:.2f} hp95={row['horizontal_p95_m']:.2f} "
|
||||
f"hmax={row['horizontal_max_m']:.2f} d3p95={row['distance_3d_p95_m']:.2f} "
|
||||
f"valid={row['valid_rate_pct']:.1f}% fde={row['fde_rate_pct']:.1f}%"
|
||||
)
|
||||
print(f"wrote {args.output}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user