Add IMU frame loss analysis script

scripts/analyze_imu_frame_loss.py

@@ -0,0 +1,121 @@
+"""Estimate missing IMU frames from sensor uptime and CSV ODR metadata."""
+
+from __future__ import annotations
+
+import argparse
+from dataclasses import dataclass
+import math
+from pathlib import Path
+import re
+
+from scripts.run_imu_ekf import read_imu_csv
+
+
+RESTART_PREVIOUS_MIN_S = 1.0
+RESTART_CURRENT_MAX_S = 0.01
+ODR_HZ_PATTERN = re.compile(r"(?P<hz>\d+(?:\.\d+)?)\s*Hz\b", re.IGNORECASE)
+
+
+@dataclass(frozen=True)
+class FrameLossResult:
+    input_csv: Path
+    odr_hz: float
+    expected_period_s: float
+    received_frames: int
+    missing_frames: int
+    segment_count: int
+
+    @property
+    def expected_frames(self) -> int:
+        return self.received_frames + self.missing_frames
+
+    @property
+    def loss_rate(self) -> float:
+        return self.missing_frames / self.expected_frames
+
+
+def analyze_file(path: Path) -> FrameLossResult:
+    path = Path(path)
+    metadata, rows = read_imu_csv(path)
+    odr_hz = _parse_odr_hz(metadata.get("odr", ""), path)
+    expected_period_s = 1.0 / odr_hz
+    tolerance_s = max(1.0e-12, expected_period_s * 1.0e-6)
+
+    received_frames = 0
+    missing_frames = 0
+    segment_count = 0
+    previous_time: float | None = None
+
+    for data_row_index, row in enumerate(rows, start=1):
+        current_time = row.sensor_uptime_s
+        received_frames += 1
+        if previous_time is None:
+            segment_count = 1
+            previous_time = current_time
+            continue
+
+        if current_time < previous_time:
+            if _is_device_restart(previous_time, current_time):
+                segment_count += 1
+                previous_time = current_time
+                continue
+            raise ValueError(
+                f"{path} timestamp decreased at data row {data_row_index}: "
+                f"previous {previous_time}, current {current_time}"
+            )
+
+        delta_s = current_time - previous_time
+        period_count = round(delta_s / expected_period_s)
+        if period_count < 1 or abs(delta_s - period_count * expected_period_s) > tolerance_s:
+            raise ValueError(
+                f"{path} timestamp gap at data row {data_row_index} is not aligned to ODR: "
+                f"delta {delta_s}, expected period {expected_period_s}"
+            )
+        missing_frames += period_count - 1
+        previous_time = current_time
+
+    if received_frames == 0:
+        raise ValueError(f"{path} has no IMU rows")
+
+    return FrameLossResult(
+        input_csv=path,
+        odr_hz=odr_hz,
+        expected_period_s=expected_period_s,
+        received_frames=received_frames,
+        missing_frames=missing_frames,
+        segment_count=segment_count,
+    )
+
+
+def main(argv: list[str] | None = None) -> int:
+    parser = argparse.ArgumentParser(description="Estimate missing IMU frames from sensor_uptime_s.")
+    parser.add_argument("csv_files", nargs="+", type=Path)
+    args = parser.parse_args(argv)
+
+    for path in args.csv_files:
+        result = analyze_file(path)
+        print(
+            f"{result.input_csv}: odr={result.odr_hz:g}Hz, "
+            f"received={result.received_frames}, missing={result.missing_frames}, "
+            f"expected={result.expected_frames}, loss_rate={result.loss_rate:.9%}, "
+            f"segments={result.segment_count}"
+        )
+    return 0
+
+
+def _parse_odr_hz(value: str, path: Path) -> float:
+    match = ODR_HZ_PATTERN.search(value)
+    if match is None:
+        raise ValueError(f"{path} odr metadata must contain a frequency in Hz")
+    odr_hz = float(match.group("hz"))
+    if not math.isfinite(odr_hz) or odr_hz <= 0.0:
+        raise ValueError(f"{path} odr frequency must be finite and positive")
+    return odr_hz
+
+
+def _is_device_restart(previous_time: float, current_time: float) -> bool:
+    return previous_time >= RESTART_PREVIOUS_MIN_S and current_time <= RESTART_CURRENT_MAX_S
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tests/test_analyze_imu_frame_loss.py

@@ -0,0 +1,58 @@
+import tempfile
+import unittest
+from pathlib import Path
+
+from scripts import analyze_imu_frame_loss
+
+
+class AnalyzeImuFrameLossTests(unittest.TestCase):
+    def _write_csv(self, path: Path, times: list[float], odr: str = "0x0F - 500 Hz"):
+        lines = [
+            f"# odr={odr}",
+            "sensor_uptime_s,temp_c,acc_x_g,acc_y_g,acc_z_g,gyro_x_dps,gyro_y_dps,gyro_z_dps",
+        ]
+        lines.extend(f"{time},28,0,0,1,0,0,0" for time in times)
+        path.write_text("\n".join(lines), encoding="utf-8-sig")
+
+    def test_counts_missing_frames_from_odr_period(self):
+        with tempfile.TemporaryDirectory() as tmp:
+            path = Path(tmp) / "imu.csv"
+            self._write_csv(path, [0.0, 0.002, 0.008])
+
+            result = analyze_imu_frame_loss.analyze_file(path)
+
+        self.assertEqual(result.received_frames, 3)
+        self.assertEqual(result.missing_frames, 2)
+        self.assertEqual(result.expected_frames, 5)
+        self.assertAlmostEqual(result.loss_rate, 0.4)
+        self.assertEqual(result.segment_count, 1)
+
+    def test_device_restart_starts_new_segment_without_loss(self):
+        with tempfile.TemporaryDirectory() as tmp:
+            path = Path(tmp) / "imu.csv"
+            self._write_csv(path, [1.0, 1.002, 0.0, 0.002])
+
+            result = analyze_imu_frame_loss.analyze_file(path)
+
+        self.assertEqual(result.missing_frames, 0)
+        self.assertEqual(result.segment_count, 2)
+
+    def test_non_integral_period_gap_reports_data_row(self):
+        with tempfile.TemporaryDirectory() as tmp:
+            path = Path(tmp) / "imu.csv"
+            self._write_csv(path, [0.0, 0.003])
+
+            with self.assertRaisesRegex(ValueError, "data row 2.*0.003.*0.002"):
+                analyze_imu_frame_loss.analyze_file(path)
+
+    def test_missing_odr_metadata_fails_clearly(self):
+        with tempfile.TemporaryDirectory() as tmp:
+            path = Path(tmp) / "imu.csv"
+            self._write_csv(path, [0.0, 0.002], odr="unknown")
+
+            with self.assertRaisesRegex(ValueError, "odr.*Hz"):
+                analyze_imu_frame_loss.analyze_file(path)
+
+
+if __name__ == "__main__":
+    unittest.main()