forked from zbw/yiliao2026
153 lines
4.5 KiB
Python
153 lines
4.5 KiB
Python
#!/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()
|