标定+USB摄像头启动

scripts/evaluate_gyro_bias.py

@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+
+import argparse
+import csv
+import math
+from pathlib import Path
+from typing import Iterable, List, Optional, Sequence, Tuple
+
+
+Sample = Tuple[float, float]
+
+
+def parse_float(row: dict, key: str) -> Optional[float]:
+    value = row.get(key)
+    if value in ("", None):
+        return None
+    return float(value)
+
+
+def load_series(csv_path: Path, key: str) -> List[Sample]:
+    samples: List[Sample] = []
+    with csv_path.open("r", encoding="utf-8", newline="") as f:
+        reader = csv.DictReader(f)
+        for row in reader:
+            t = parse_float(row, "elapsed_s")
+            y = parse_float(row, key)
+            if t is None or y is None:
+                continue
+            samples.append((t, y))
+    return samples
+
+
+def fit_line(samples: Sequence[Sample]) -> Tuple[float, float]:
+    n = len(samples)
+    if n < 2:
+        raise ValueError("need at least 2 valid samples")
+
+    mean_t = sum(t for t, _ in samples) / n
+    mean_y = sum(y for _, y in samples) / n
+    s_tt = sum((t - mean_t) * (t - mean_t) for t, _ in samples)
+    if s_tt == 0.0:
+        raise ValueError("all timestamps are identical")
+    s_ty = sum((t - mean_t) * (y - mean_y) for t, y in samples)
+    slope = s_ty / s_tt
+    intercept = mean_y - slope * mean_t
+    return intercept, slope
+
+
+def mean(values: Iterable[float]) -> float:
+    values = list(values)
+    if not values:
+        raise ValueError("need at least 1 value")
+    return sum(values) / len(values)
+
+
+def stddev(values: Sequence[float]) -> float:
+    if len(values) < 2:
+        return 0.0
+    mu = mean(values)
+    return math.sqrt(sum((v - mu) * (v - mu) for v in values) / len(values))
+
+
+def integrate(samples: Sequence[Sample]) -> float:
+    if len(samples) < 2:
+        return 0.0
+    total = 0.0
+    prev_t, prev_y = samples[0]
+    for t, y in samples[1:]:
+        dt = t - prev_t
+        total += 0.5 * (prev_y + y) * dt
+        prev_t, prev_y = t, y
+    return total
+
+
+def drift_rate_deg_per_min(samples: Sequence[Sample]) -> float:
+    if len(samples) < 2:
+        return 0.0
+    total_time = samples[-1][0] - samples[0][0]
+    if total_time <= 0.0:
+        return 0.0
+    yaw_drift_rad = integrate(samples)
+    return math.degrees(yaw_drift_rad) / (total_time / 60.0)
+
+
+def print_series_stats(name: str, samples: Sequence[Sample]) -> None:
+    values = [y for _, y in samples]
+    intercept, slope = fit_line(samples)
+    print(f"{name}:")
+    print(f"  samples={len(samples)}")
+    print(f"  mean={mean(values):.12f} rad/s")
+    print(f"  stddev={stddev(values):.12f} rad/s")
+    print(f"  line_intercept={intercept:.12f} rad/s")
+    print(f"  line_slope={slope:.12f} rad/s^2")
+    print(f"  integrated_drift={integrate(samples):.12f} rad ({math.degrees(integrate(samples)):.6f} deg)")
+    print(f"  drift_rate={drift_rate_deg_per_min(samples):.6f} deg/min")
+
+
+def print_yaw_span(name: str, samples: Sequence[Sample]) -> None:
+    if len(samples) < 2:
+        return
+    yaw_delta = samples[-1][1] - samples[0][1]
+    total_time = samples[-1][0] - samples[0][0]
+    print(f"{name}:")
+    print(f"  start={samples[0][1]:.12f} rad")
+    print(f"  end={samples[-1][1]:.12f} rad")
+    print(f"  delta={yaw_delta:.12f} rad ({math.degrees(yaw_delta):.6f} deg)")
+    if total_time > 0.0:
+        print(f"  rate={math.degrees(yaw_delta) / (total_time / 60.0):.6f} deg/min")
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Evaluate gyro bias compensation quality from feedback.csv")
+    parser.add_argument("csv_path", type=Path, help="path to feedback.csv")
+    args = parser.parse_args()
+
+    pre_bias = load_series(args.csv_path, "gyro_z_filtered_pre_bias")
+    bias_model = load_series(args.csv_path, "gyro_z_bias_model")
+    final_for_yaw = load_series(args.csv_path, "gyro_z_final_for_yaw")
+    odom_yaw = load_series(args.csv_path, "odom_yaw_rad")
+    imu_yaw = load_series(args.csv_path, "imu_yaw_rad")
+
+    if pre_bias:
+        print_series_stats("gyro_z_filtered_pre_bias", pre_bias)
+        print()
+    else:
+        print("gyro_z_filtered_pre_bias: no valid samples")
+        print()
+
+    if bias_model:
+        print_series_stats("gyro_z_bias_model", bias_model)
+        print()
+    else:
+        print("gyro_z_bias_model: no valid samples")
+        print()
+
+    if final_for_yaw:
+        print_series_stats("gyro_z_final_for_yaw", final_for_yaw)
+        print()
+    else:
+        print("gyro_z_final_for_yaw: no valid samples")
+        print()
+
+    if odom_yaw:
+        print_yaw_span("odom_yaw_rad", odom_yaw)
+        print()
+
+    if imu_yaw:
+        print_yaw_span("imu_yaw_rad", imu_yaw)
+
+
+if __name__ == "__main__":
+    main()