新增独立RTK与IMU外参标定流程及质量验证
This commit is contained in:
lichun.qu
@@ -0,0 +1,308 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Cut G90 captures into per-window RTK CSV files.
|
||||
|
||||
NMEA GGA/GNHPR UTC is the measurement time. Host receive UTC is retained only
|
||||
for diagnostics. The measurement UTC is mapped onto the IMU device clock with
|
||||
one robust affine clock model per window.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from datetime import datetime, timedelta, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from tools.h32_dlog.timeutil import utc_dotnet_ticks_to_unix_s
|
||||
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||
from tools.rscap_v2.g90_rtk import RtkSentence, iter_g90_sentences
|
||||
from tools.time_alignment import AffineClockModel, fit_affine_clock
|
||||
|
||||
LOCAL_TZ = timezone(timedelta(hours=8))
|
||||
HPR_MATCH_S = 0.08
|
||||
CSV_FIELDS = [
|
||||
"t",
|
||||
"t_measurement_utc_s",
|
||||
"t_host_utc_s",
|
||||
"receive_delay_s",
|
||||
"t_local",
|
||||
"receive_utc_ticks",
|
||||
"lat_deg",
|
||||
"lon_deg",
|
||||
"altitude_m",
|
||||
"fix_quality",
|
||||
"satellites",
|
||||
"hdop",
|
||||
"heading_deg",
|
||||
"pitch_deg",
|
||||
"roll_deg",
|
||||
"heading_quality",
|
||||
"heading_valid",
|
||||
"gga_utc",
|
||||
"hpr_utc",
|
||||
"hpr_measurement_utc_s",
|
||||
"checksum_valid",
|
||||
]
|
||||
|
||||
|
||||
def _fmt(value) -> str:
|
||||
if value is None:
|
||||
return ""
|
||||
if isinstance(value, bool):
|
||||
return "1" if value else "0"
|
||||
if isinstance(value, float):
|
||||
if math.isnan(value):
|
||||
return ""
|
||||
return f"{value:.12g}"
|
||||
return str(value)
|
||||
|
||||
|
||||
def load_imu_clock_model(
|
||||
imu_csv: Path,
|
||||
) -> tuple[float, float, float, float, AffineClockModel]:
|
||||
"""Return device/host spans and a robust ``IMU device -> host UTC`` model."""
|
||||
|
||||
t_device: list[float] = []
|
||||
t_host: list[float] = []
|
||||
with imu_csv.open("r", encoding="utf-8", newline="") as handle:
|
||||
reader = csv.DictReader(handle)
|
||||
for row in reader:
|
||||
t_device.append(float(row["t"]))
|
||||
t_host.append(float(row["t_host_utc_s"]))
|
||||
if not t_host:
|
||||
raise ValueError(f"empty IMU csv: {imu_csv}")
|
||||
model = fit_affine_clock(np.asarray(t_device), np.asarray(t_host))
|
||||
return min(t_device), max(t_device), min(t_host), max(t_host), model
|
||||
|
||||
|
||||
def sentence_host_s(row: RtkSentence) -> float:
|
||||
return utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
|
||||
|
||||
|
||||
def nmea_utc_to_unix_s(value: str | None, receive_host_s: float) -> float:
|
||||
"""Resolve NMEA ``hhmmss.s`` to the UTC day nearest host receive time."""
|
||||
|
||||
if value is None or not str(value).strip():
|
||||
raise ValueError("missing NMEA UTC time")
|
||||
packed = float(value)
|
||||
hour = int(packed // 10000)
|
||||
minute = int((packed - hour * 10000) // 100)
|
||||
second = packed - hour * 10000 - minute * 100
|
||||
if not (0 <= hour < 24 and 0 <= minute < 60 and 0.0 <= second < 60.0):
|
||||
raise ValueError(f"invalid NMEA UTC time: {value!r}")
|
||||
receive = datetime.fromtimestamp(receive_host_s, tz=timezone.utc)
|
||||
midnight = datetime(
|
||||
receive.year,
|
||||
receive.month,
|
||||
receive.day,
|
||||
tzinfo=timezone.utc,
|
||||
).timestamp()
|
||||
same_day = midnight + hour * 3600 + minute * 60 + second
|
||||
return min(
|
||||
(same_day - 86400.0, same_day, same_day + 86400.0),
|
||||
key=lambda candidate: abs(candidate - receive_host_s),
|
||||
)
|
||||
|
||||
|
||||
def sentence_measurement_utc_s(row: RtkSentence) -> float:
|
||||
return nmea_utc_to_unix_s(
|
||||
row.fields.get("position_time_utc"),
|
||||
sentence_host_s(row),
|
||||
)
|
||||
|
||||
|
||||
def nearest_hpr(gga: RtkSentence, hpr_rows: list[RtkSentence]) -> RtkSentence | None:
|
||||
if not hpr_rows:
|
||||
return None
|
||||
lo, hi = 0, len(hpr_rows) - 1
|
||||
target = sentence_measurement_utc_s(gga)
|
||||
while lo < hi:
|
||||
mid = (lo + hi) // 2
|
||||
if sentence_measurement_utc_s(hpr_rows[mid]) < target:
|
||||
lo = mid + 1
|
||||
else:
|
||||
hi = mid
|
||||
best = hpr_rows[lo]
|
||||
if lo > 0 and abs(sentence_measurement_utc_s(hpr_rows[lo - 1]) - target) < abs(
|
||||
sentence_measurement_utc_s(best) - target
|
||||
):
|
||||
best = hpr_rows[lo - 1]
|
||||
if abs(sentence_measurement_utc_s(best) - target) > HPR_MATCH_S:
|
||||
return None
|
||||
return best
|
||||
|
||||
|
||||
def merged_row(
|
||||
gga: RtkSentence,
|
||||
hpr: RtkSentence | None,
|
||||
imu_to_host: AffineClockModel,
|
||||
) -> dict:
|
||||
t_host = sentence_host_s(gga)
|
||||
t_measurement = sentence_measurement_utc_s(gga)
|
||||
t_hpr = None if hpr is None else sentence_measurement_utc_s(hpr)
|
||||
local = datetime.fromtimestamp(t_measurement, tz=timezone.utc).astimezone(LOCAL_TZ)
|
||||
fields = gga.fields
|
||||
hpr_fields = hpr.fields if hpr is not None else {}
|
||||
checksum = gga.checksum_valid and (hpr is None or hpr.checksum_valid)
|
||||
return {
|
||||
"t": imu_to_host.inverse(t_measurement),
|
||||
"t_measurement_utc_s": t_measurement,
|
||||
"t_host_utc_s": t_host,
|
||||
"receive_delay_s": t_host - t_measurement,
|
||||
"t_local": local.strftime("%Y-%m-%dT%H:%M:%S.%f")[:-3],
|
||||
"receive_utc_ticks": gga.receive_utc_ticks,
|
||||
"lat_deg": fields.get("lat_deg"),
|
||||
"lon_deg": fields.get("lon_deg"),
|
||||
"altitude_m": fields.get("altitude_m"),
|
||||
"fix_quality": fields.get("fix_quality"),
|
||||
"satellites": fields.get("satellites"),
|
||||
"hdop": fields.get("hdop"),
|
||||
"heading_deg": hpr_fields.get("heading_deg"),
|
||||
"pitch_deg": hpr_fields.get("pitch_deg"),
|
||||
"roll_deg": hpr_fields.get("roll_deg"),
|
||||
"heading_quality": hpr_fields.get("heading_quality"),
|
||||
"heading_valid": hpr_fields.get("heading_valid"),
|
||||
"gga_utc": fields.get("position_time_utc"),
|
||||
"hpr_utc": hpr_fields.get("position_time_utc"),
|
||||
"hpr_measurement_utc_s": t_hpr,
|
||||
"checksum_valid": checksum,
|
||||
}
|
||||
|
||||
|
||||
def write_rtk_csv(path: Path, rows: list[dict]) -> None:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.DictWriter(handle, fieldnames=CSV_FIELDS)
|
||||
writer.writeheader()
|
||||
for row in rows:
|
||||
writer.writerow({key: _fmt(row[key]) for key in CSV_FIELDS})
|
||||
|
||||
|
||||
def discover_windows(sessions_root: Path) -> list[Path]:
|
||||
windows = sorted(
|
||||
path
|
||||
for path in sessions_root.iterdir()
|
||||
if path.is_dir() and (path / "imu.csv").is_file()
|
||||
)
|
||||
if not windows:
|
||||
raise SystemExit(f"no session dirs with imu.csv under {sessions_root}")
|
||||
return windows
|
||||
|
||||
|
||||
def find_default_rscap(sessions_root: Path) -> Path:
|
||||
parents = [sessions_root, sessions_root.parent]
|
||||
matches: list[Path] = []
|
||||
for folder in parents:
|
||||
matches.extend(sorted(folder.glob("wheeltec-g90*.rscap")))
|
||||
matches.extend(sorted(folder.glob("*g90*.rscap")))
|
||||
if not matches:
|
||||
raise SystemExit(f"no G90 .rscap next to {sessions_root}")
|
||||
return matches[0]
|
||||
|
||||
|
||||
def _delay_summary(rows: list[dict]) -> dict[str, float] | None:
|
||||
if not rows:
|
||||
return None
|
||||
values = np.asarray([row["receive_delay_s"] for row in rows], dtype=np.float64)
|
||||
return {
|
||||
"median_s": float(np.median(values)),
|
||||
"p05_s": float(np.percentile(values, 5.0)),
|
||||
"p95_s": float(np.percentile(values, 95.0)),
|
||||
"std_s": float(np.std(values)),
|
||||
}
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--sessions-root", type=Path, required=True)
|
||||
parser.add_argument("--rtk-rscap", type=Path, action="append")
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
args = parser.parse_args(argv)
|
||||
sessions_root = args.sessions_root.resolve()
|
||||
rscaps = [path.resolve() for path in (args.rtk_rscap or [find_default_rscap(sessions_root)])]
|
||||
for rscap in rscaps:
|
||||
if not rscap.is_file():
|
||||
raise SystemExit(f"missing RTK capture: {rscap}")
|
||||
|
||||
sentences: list[RtkSentence] = []
|
||||
captures_meta = []
|
||||
for rscap in rscaps:
|
||||
print(f"reading {rscap}", flush=True)
|
||||
capture = read_capture(rscap)
|
||||
print(f"chunks={len(capture.chunks)}", flush=True)
|
||||
sentences.extend(iter_g90_sentences(capture))
|
||||
captures_meta.append(file_summary(capture))
|
||||
sentences.sort(key=lambda row: row.receive_utc_ticks)
|
||||
gga_all = [row for row in sentences if row.sentence_type == "GGA"]
|
||||
hpr_all = [row for row in sentences if row.sentence_type == "GNHPR"]
|
||||
hpr_all.sort(key=sentence_measurement_utc_s)
|
||||
print(
|
||||
f"parsed sentences={len(sentences)} GGA={len(gga_all)} GNHPR={len(hpr_all)}",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
summaries = {
|
||||
"rtk_rscap": [str(path) for path in rscaps],
|
||||
"captures": captures_meta,
|
||||
"parsed_sentences": len(sentences),
|
||||
"gga": len(gga_all),
|
||||
"gnhpr": len(hpr_all),
|
||||
"time_source": "NMEA measurement UTC mapped through IMU device->host affine clock",
|
||||
"windows": [],
|
||||
}
|
||||
for window in discover_windows(sessions_root):
|
||||
out_csv = window / "rtk.csv"
|
||||
if out_csv.exists() and not args.overwrite:
|
||||
raise SystemExit(f"{out_csv} exists; pass --overwrite")
|
||||
dev_min, dev_max, host_min, host_max, imu_to_host = load_imu_clock_model(
|
||||
window / "imu.csv"
|
||||
)
|
||||
gga = [
|
||||
row
|
||||
for row in gga_all
|
||||
if dev_min <= imu_to_host.inverse(sentence_measurement_utc_s(row)) <= dev_max
|
||||
]
|
||||
hpr = [
|
||||
row
|
||||
for row in hpr_all
|
||||
if dev_min - HPR_MATCH_S
|
||||
<= imu_to_host.inverse(sentence_measurement_utc_s(row))
|
||||
<= dev_max + HPR_MATCH_S
|
||||
]
|
||||
merged = [merged_row(row, nearest_hpr(row, hpr), imu_to_host) for row in gga]
|
||||
write_rtk_csv(out_csv, merged)
|
||||
brief = {
|
||||
"window": window.name,
|
||||
"imu_host_span_s": [host_min, host_max],
|
||||
"imu_device_span_s": [dev_min, dev_max],
|
||||
"imu_device_to_host_clock": imu_to_host.to_dict(),
|
||||
"rtk_receive_delay": _delay_summary(merged),
|
||||
"gga": len(gga),
|
||||
"gnhpr_in_window": len(hpr),
|
||||
"rows_written": len(merged),
|
||||
"heading_matched": sum(1 for row in merged if row["heading_deg"] is not None),
|
||||
"fix_quality_4_or_5": sum(
|
||||
1 for row in merged if row["fix_quality"] in {4, 5}
|
||||
),
|
||||
"out": str(out_csv),
|
||||
}
|
||||
summaries["windows"].append(brief)
|
||||
print(json.dumps(brief, ensure_ascii=False), flush=True)
|
||||
|
||||
manifest = sessions_root / "rtk_export_summary.json"
|
||||
manifest.write_text(json.dumps(summaries, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
||||
print(f"summary: {manifest}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,145 @@
|
||||
"""Decode Wheeltec G90 NMEA (GGA / GNHPR) from a V2 .rscap capture."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import bisect
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
|
||||
|
||||
def nmea_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1 : star + 3], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
value = 0
|
||||
for char in line[1:star]:
|
||||
value ^= ord(char)
|
||||
return value == expected
|
||||
|
||||
|
||||
def _safe_float(value: str):
|
||||
try:
|
||||
return float(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def _safe_int(value: str):
|
||||
try:
|
||||
return int(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def parse_nmea_latlon(value: str, hemisphere: str):
|
||||
raw = _safe_float(value)
|
||||
if raw is None:
|
||||
return None
|
||||
degrees = math.floor(raw / 100.0)
|
||||
result = degrees + (raw - degrees * 100.0) / 60.0
|
||||
if hemisphere.upper() in ("S", "W"):
|
||||
result = -result
|
||||
return result
|
||||
|
||||
|
||||
def parse_gga(line: str) -> dict:
|
||||
fields = line[: line.rfind("*")].split(",")
|
||||
if len(fields) < 10:
|
||||
raise ValueError("GGA has too few fields")
|
||||
return {
|
||||
"type": "GGA",
|
||||
"position_time_utc": fields[1],
|
||||
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
|
||||
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
|
||||
"fix_quality": _safe_int(fields[6]),
|
||||
"satellites": _safe_int(fields[7]),
|
||||
"hdop": _safe_float(fields[8]),
|
||||
"altitude_m": _safe_float(fields[9]),
|
||||
}
|
||||
|
||||
|
||||
def parse_gnhpr(line: str) -> dict:
|
||||
fields = line[: line.rfind("*")].split(",")
|
||||
if len(fields) < 7:
|
||||
raise ValueError("GNHPR has too few fields")
|
||||
quality = _safe_int(fields[5])
|
||||
return {
|
||||
"type": "GNHPR",
|
||||
"position_time_utc": fields[1],
|
||||
"heading_deg": _safe_float(fields[2]),
|
||||
"pitch_deg": _safe_float(fields[3]),
|
||||
"roll_deg": _safe_float(fields[4]),
|
||||
"heading_quality": quality,
|
||||
"satellites": _safe_int(fields[6]),
|
||||
"heading_valid": quality in {4, 5},
|
||||
}
|
||||
|
||||
|
||||
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
|
||||
starts = []
|
||||
cursor = 0
|
||||
for chunk in chunks:
|
||||
starts.append(cursor)
|
||||
cursor += len(chunk.raw)
|
||||
return starts
|
||||
|
||||
|
||||
def _host_ticks_for_span(chunks: list[RawChunk], starts: list[int], end: int) -> int:
|
||||
end_index = max(0, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
|
||||
return chunks[end_index].receive_utc_ticks
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RtkSentence:
|
||||
sentence_type: str
|
||||
receive_utc_ticks: int
|
||||
checksum_valid: bool
|
||||
fields: dict
|
||||
raw_line: str
|
||||
|
||||
|
||||
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
"""Parse GGA/GNHPR lines; host time comes from the containing serial chunk."""
|
||||
|
||||
rows: list[RtkSentence] = []
|
||||
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
starts = _chunk_starts(chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
if not (line.startswith("$GNGGA") or line.startswith("$GPGGA") or line.startswith("$GNHPR")):
|
||||
continue
|
||||
ticks = _host_ticks_for_span(chunks, starts, end)
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
fields = parse_gga(line)
|
||||
else:
|
||||
fields = parse_gnhpr(line)
|
||||
except ValueError:
|
||||
continue
|
||||
rows.append(
|
||||
RtkSentence(
|
||||
sentence_type=str(fields["type"]),
|
||||
receive_utc_ticks=int(ticks),
|
||||
checksum_valid=nmea_checksum_valid(line),
|
||||
fields=fields,
|
||||
raw_line=line,
|
||||
)
|
||||
)
|
||||
rows.sort(key=lambda row: row.receive_utc_ticks)
|
||||
return rows
|
||||
@@ -0,0 +1,93 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run the independent RTK--IMU calibration against the project inventory."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_replay import load_inventory, load_sessions, run_calibration
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument(
|
||||
"--inventory",
|
||||
type=Path,
|
||||
default=ROOT / "artifacts" / "rtk_imu_inventory_v1" / "rtk_session_inventory.csv",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output-dir",
|
||||
type=Path,
|
||||
default=ROOT / "artifacts" / "rtk_imu_calibration_v1",
|
||||
)
|
||||
parser.add_argument("--session", action="append", help="session id to include; repeatable")
|
||||
parser.add_argument("--batch", action="append", help="batch id to include; repeatable")
|
||||
parser.add_argument("--rotation-only", action="store_true")
|
||||
parser.add_argument("--no-loo", action="store_true")
|
||||
parser.add_argument("--knot-step-s", type=float, default=2.0)
|
||||
parser.add_argument("--per-batch", action="store_true")
|
||||
parser.add_argument("--rtk-frame-definition", default='')
|
||||
parser.add_argument("--rtk-reference-point", default='')
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
entries = load_inventory(args.inventory)
|
||||
if args.session:
|
||||
selected = set(args.session)
|
||||
entries = [entry for entry in entries if entry.session_id in selected]
|
||||
if args.batch:
|
||||
selected_batches = set(args.batch)
|
||||
entries = [entry for entry in entries if entry.batch_id in selected_batches]
|
||||
if not entries:
|
||||
raise SystemExit("no inventory rows match the requested selection")
|
||||
sessions = load_sessions(entries)
|
||||
rotation, translation = run_calibration(
|
||||
sessions,
|
||||
args.output_dir,
|
||||
rotation_only=args.rotation_only,
|
||||
compute_loo=not args.no_loo,
|
||||
knot_step_s=args.knot_step_s,
|
||||
rtk_frame_definition=args.rtk_frame_definition,
|
||||
rtk_reference_point=args.rtk_reference_point,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"output": str(args.output_dir.resolve()),
|
||||
"sessions": [session.session_id for session in sessions],
|
||||
"rotation_ok": rotation.ok,
|
||||
"rotation_rpy_deg": rotation.rpy_deg.tolist(),
|
||||
"rotation_rms_deg": rotation.residual_rms_deg,
|
||||
"translation_ok": None if translation is None else translation.ok,
|
||||
"translation_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
|
||||
},
|
||||
ensure_ascii=False,
|
||||
indent=2,
|
||||
)
|
||||
)
|
||||
|
||||
if args.per_batch:
|
||||
for batch in sorted({entry.batch_id for entry in entries}):
|
||||
batch_entries = [entry for entry in entries if entry.batch_id == batch]
|
||||
if len(batch_entries) < 2:
|
||||
continue
|
||||
run_calibration(
|
||||
load_sessions(batch_entries),
|
||||
args.output_dir / "per_batch" / batch,
|
||||
rotation_only=args.rotation_only,
|
||||
compute_loo=False,
|
||||
knot_step_s=args.knot_step_s,
|
||||
rtk_frame_definition=args.rtk_frame_definition,
|
||||
rtk_reference_point=args.rtk_reference_point,
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,97 @@
|
||||
"""Small, dependency-light helpers for mapping independent sensor clocks."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AffineClockModel:
|
||||
"""Numerically stable affine map ``y = y_ref + scale * (x - x_ref)``."""
|
||||
|
||||
x_ref: float
|
||||
y_ref: float
|
||||
scale: float
|
||||
sample_count: int
|
||||
inlier_count: int
|
||||
residual_std_s: float
|
||||
residual_p95_s: float
|
||||
|
||||
def map(self, value: float | np.ndarray) -> float | np.ndarray:
|
||||
array = np.asarray(value, dtype=np.float64)
|
||||
mapped = self.y_ref + self.scale * (array - self.x_ref)
|
||||
return float(mapped) if array.ndim == 0 else mapped
|
||||
|
||||
def inverse(self, value: float | np.ndarray) -> float | np.ndarray:
|
||||
if abs(self.scale) < 1e-12:
|
||||
raise ValueError("clock model scale is zero")
|
||||
array = np.asarray(value, dtype=np.float64)
|
||||
mapped = self.x_ref + (array - self.y_ref) / self.scale
|
||||
return float(mapped) if array.ndim == 0 else mapped
|
||||
|
||||
def to_dict(self) -> dict[str, float | int]:
|
||||
return asdict(self)
|
||||
|
||||
|
||||
def fit_affine_clock(
|
||||
x: np.ndarray,
|
||||
y: np.ndarray,
|
||||
*,
|
||||
max_iterations: int = 4,
|
||||
min_residual_gate_s: float = 5e-4,
|
||||
) -> AffineClockModel:
|
||||
"""Robustly fit an affine clock map while rejecting receive-time spikes.
|
||||
|
||||
``x`` and ``y`` may have large, unrelated epochs. Centering around their
|
||||
medians avoids losing precision when host UTC is around 1e9 seconds.
|
||||
"""
|
||||
|
||||
x_values = np.asarray(x, dtype=np.float64).reshape(-1)
|
||||
y_values = np.asarray(y, dtype=np.float64).reshape(-1)
|
||||
finite = np.isfinite(x_values) & np.isfinite(y_values)
|
||||
x_values = x_values[finite]
|
||||
y_values = y_values[finite]
|
||||
if x_values.size < 2:
|
||||
raise ValueError("need at least two finite clock samples")
|
||||
|
||||
x_ref = float(np.median(x_values))
|
||||
y_ref = float(np.median(y_values))
|
||||
dx = x_values - x_ref
|
||||
dy = y_values - y_ref
|
||||
inliers = np.ones(x_values.size, dtype=bool)
|
||||
scale = 1.0
|
||||
offset = 0.0
|
||||
|
||||
for _ in range(max_iterations):
|
||||
local_x = dx[inliers]
|
||||
local_y = dy[inliers]
|
||||
denom = float(local_x @ local_x)
|
||||
if denom < 1e-18:
|
||||
raise ValueError("clock samples do not span enough time")
|
||||
scale = float(local_x @ local_y / denom)
|
||||
offset = float(np.median(local_y - scale * local_x))
|
||||
residual = dy - (offset + scale * dx)
|
||||
center = float(np.median(residual[inliers]))
|
||||
mad = float(np.median(np.abs(residual[inliers] - center)))
|
||||
sigma = 1.4826 * mad
|
||||
gate = max(float(min_residual_gate_s), 6.0 * sigma)
|
||||
updated = np.abs(residual - center) <= gate
|
||||
if np.count_nonzero(updated) < 2 or np.array_equal(updated, inliers):
|
||||
break
|
||||
inliers = updated
|
||||
|
||||
# Fold the small centered intercept into y_ref so map/inverse stay simple.
|
||||
y_ref += offset
|
||||
residual = y_values - (y_ref + scale * (x_values - x_ref))
|
||||
residual_inliers = residual[inliers]
|
||||
return AffineClockModel(
|
||||
x_ref=x_ref,
|
||||
y_ref=y_ref,
|
||||
scale=scale,
|
||||
sample_count=int(x_values.size),
|
||||
inlier_count=int(np.count_nonzero(inliers)),
|
||||
residual_std_s=float(np.std(residual_inliers)),
|
||||
residual_p95_s=float(np.percentile(np.abs(residual_inliers), 95.0)),
|
||||
)
|
||||
Reference in New Issue
Block a user