From be7c498270216015c4c4e971e5b78b467cb8e51d Mon Sep 17 00:00:00 2001 From: Brunsmeier <2970937094@qq.com> Date: Thu, 2 Jul 2026 11:19:53 +0800 Subject: [PATCH] Add verification script for IMU gravity compensation and velocity integration --- README.md | 57 +++++- verify.py | 570 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 624 insertions(+), 3 deletions(-) create mode 100644 verify.py diff --git a/README.md b/README.md index b516060..bf50c5b 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,7 @@ This project bridges IMU data through an ESP32 to a PC, with optional gripper/re - `c`: CLOSE - `s`: STOP - Generates statistics, CSV files, and plots from either live serial data or saved text logs. +- Estimates world-frame linear acceleration and velocity from IMU acceleration and Euler angles. - Uses a physical switch on GPIO27 to trigger one-shot gripper open/close pulses. ## Files @@ -20,7 +21,7 @@ This project bridges IMU data through an ESP32 to a PC, with optional gripper/re | `main.py` | Main MicroPython program for the ESP32. It reads the IMU, controls DIO, and streams USB serial packets. | | `esp_bridge.py` | PC-side Python wrapper class for integrating the ESP32 bridge into other programs. | | `pc_reader.py` | PC-side real-time serial reader with keyboard gripper commands. | -| `visualise.py` | Sampling, statistics, CSV export, and plotting tool. | +| `visualise.py` | Sampling, statistics, CSV export, plotting, linear-acceleration estimation, and velocity integration tool. | | `test.py` | ESP32 UART pin scan/debug helper. | | `imu_data.csv` | Example or exported IMU sample data. | | `rcd.txt` | Example text log from `pc_reader.py`. | @@ -172,12 +173,62 @@ Read from a `pc_reader.py` text log and plot it: python3 visualise.py --file rcd.txt --output imu_quality.png ``` -If the output file already exists, `visualise.py` appends a timestamp to avoid overwriting it, for example: +`visualise.py` now adds derived motion columns before saving or plotting: + +| Column | Description | +| --- | --- | +| `lin_acc_x_ms2`, `lin_acc_y_ms2`, `lin_acc_z_ms2` | Estimated world-frame linear acceleration in `m/s^2` after gravity and bias compensation. | +| `vel_x_ms`, `vel_y_ms`, `vel_z_ms` | Estimated world-frame velocity in `m/s` from integrating linear acceleration. | + +The generated plot contains six stacked views: + +- raw acceleration +- linear acceleration +- velocity +- gyro +- angle +- packet frequency + +CSV files are saved under `csv/`, PNG files are saved under `png/`, and other output extensions are saved under `output/`. If the target file already exists, `visualise.py` appends a timestamp to avoid overwriting it, for example: ```text -imu_quality_20260701_131902.png +png/imu_quality_20260701_131902.png ``` +## Linear Acceleration And Velocity + +The raw accelerometer output includes both motion acceleration and the support-force/gravity-related acceleration measured by the IMU. When the sensor is placed horizontally and kept still, it will still measure about `1g` on the vertical axis. This is expected: the table support force prevents free fall, and the accelerometer senses that proper acceleration. + +Because of this, `visualise.py` removes the static `1g` component before integrating velocity. The current calculation is: + +1. Read raw acceleration in the IMU/body frame: `acc_x_g`, `acc_y_g`, `acc_z_g`. +2. Convert Euler angles to a body-to-world rotation matrix: + + ```python + R = Rz(yaw) @ Ry(pitch) @ Rx(roll) + ``` + +3. Rotate body-frame acceleration into the world frame: + + ```python + acc_world_g = R @ acc_body_g + ``` + +4. Subtract the world Z-axis support-force/gravity component: + + ```python + linear_acc_world_g = acc_world_g - [0, 0, gravity_sign] + ``` + + The default call uses `gravity_sign=1.0`, so a horizontally placed, still sensor is expected to have approximately `+1g` on the world Z axis before compensation. + +5. Convert from `g` to `m/s^2` with `9.81`. +6. Estimate a small acceleration bias from the first `bias_seconds` seconds, default `1.0` second, and subtract it. +7. Apply an acceleration deadband, default `0.15 m/s^2`, to reduce small stationary noise. +8. Integrate acceleration to velocity with a small decay factor, default `vel_decay=0.995`, to limit drift. + +This velocity estimate is useful for short-duration motion checks and relative comparisons. It will drift over time because low-cost IMU acceleration, attitude error, and numerical integration all accumulate error. Keep the sensor still for the first second when possible so the initial bias estimate is meaningful. + ## Packet Protocol The ESP32-to-PC binary packet format is defined in `main.py`, `pc_reader.py`, and `esp_bridge.py`: diff --git a/verify.py b/verify.py new file mode 100644 index 0000000..735cc9a --- /dev/null +++ b/verify.py @@ -0,0 +1,570 @@ +import argparse +import ast +import csv +import math +import os +import re +import sys +import time +from datetime import datetime + +os.environ.setdefault("MPLCONFIGDIR", "/tmp/matplotlib") + +import matplotlib.pyplot as plt +import numpy as np +import serial + +from pc_reader import PACKET_SIZE, SYNC, decode_packet + + +TEXT_LINE_RE = re.compile( + r"#?\s*(?P\d+)\s+" + r"(?P[-+]?\d+(?:\.\d+)?)Hz\s+" + r"DIN=(?P\[[^\]]+\])\s+" + r"DOUT=(?P\[[^\]]+\])\s+" + r"ACC=(?P\[[^\]]+\])\s+" + r"GYRO=(?P\[[^\]]+\])\s+" + r"ANGLE=(?P\[[^\]]+\])" +) + + +def flatten_packet(data, t_s, freq_hz): + return { + "t_s": t_s, + "acc_x_g": data["acc_g"][0], + "acc_y_g": data["acc_g"][1], + "acc_z_g": data["acc_g"][2], + "gyro_x_dps": data["gyro_dps"][0], + "gyro_y_dps": data["gyro_dps"][1], + "gyro_z_dps": data["gyro_dps"][2], + "angle_x_deg": data["angle_deg"][0], + "angle_y_deg": data["angle_deg"][1], + "angle_z_deg": data["angle_deg"][2], + "temp_c": data["temp_c"], + "freq_hz": freq_hz, + "din0": data["din"][0], + "din1": data["din"][1], + "din2": data["din"][2], + "din3": data["din"][3], + "dout0": data["dout"][0], + "dout1": data["dout"][1], + } + + +def read_serial_samples(port, baud, seconds=None, max_samples=None, max_gap_ms=1000): + samples = [] + buffer = bytearray() + last_t_ms = None + elapsed_s = 0.0 + bad_delta_count = 0 + start = time.monotonic() + + with serial.Serial(port, baud, timeout=1) as ser: + print(f"Reading {port} at {baud} baud...", file=sys.stderr) + + while True: + if seconds is not None and time.monotonic() - start >= seconds: + break + if max_samples is not None and len(samples) >= max_samples: + break + + chunk = ser.read(ser.in_waiting or 1) + if chunk: + buffer.extend(chunk) + + while len(buffer) >= PACKET_SIZE: + sync_index = buffer.find(SYNC) + if sync_index < 0: + del buffer[:-1] + break + + if sync_index: + del buffer[:sync_index] + + if len(buffer) < PACKET_SIZE: + break + + packet = bytes(buffer[:PACKET_SIZE]) + + if (sum(packet[:-1]) & 0xFF) != packet[-1]: + del buffer[0] + continue + + del buffer[:PACKET_SIZE] + data = decode_packet(packet) + + if last_t_ms is None: + freq_hz = 0.0 + else: + delta = (data["t_ms"] - last_t_ms) & 0xFFFFFFFF + if 0 < delta <= max_gap_ms: + elapsed_s += delta / 1000.0 + freq_hz = 1000.0 / delta + else: + bad_delta_count += 1 + freq_hz = 0.0 + + last_t_ms = data["t_ms"] + samples.append(flatten_packet(data, elapsed_s, freq_hz)) + + if max_samples is not None and len(samples) >= max_samples: + break + + if bad_delta_count: + print(f"Ignored {bad_delta_count} abnormal packet time delta(s).", file=sys.stderr) + + return samples + + +def read_text_log(path): + samples = [] + t_s = 0.0 + + with open(path, "r", encoding="utf-8") as f: + for line in f: + match = TEXT_LINE_RE.search(line) + if not match: + continue + + freq_hz = float(match.group("freq")) + din = ast.literal_eval(match.group("din")) + dout = ast.literal_eval(match.group("dout")) + acc = ast.literal_eval(match.group("acc")) + gyro = ast.literal_eval(match.group("gyro")) + angle = ast.literal_eval(match.group("angle")) + + if samples and freq_hz > 0: + t_s += 1.0 / freq_hz + + samples.append( + { + "t_s": t_s, + "acc_x_g": acc[0], + "acc_y_g": acc[1], + "acc_z_g": acc[2], + "gyro_x_dps": gyro[0], + "gyro_y_dps": gyro[1], + "gyro_z_dps": gyro[2], + "angle_x_deg": angle[0], + "angle_y_deg": angle[1], + "angle_z_deg": angle[2], + "temp_c": np.nan, + "freq_hz": freq_hz, + "din0": din[0], + "din1": din[1], + "din2": din[2], + "din3": din[3], + "dout0": dout[0], + "dout1": dout[1], + } + ) + + return samples + + +def verify_gravity_body_g(roll_deg, pitch_deg, gravity_sign=1.0): + """ + Provider's roll/pitch gravity projection. + + Assumption: + roll = angle_x_deg + pitch = angle_y_deg + yaw is ignored. + + gravity_sign: + +1.0 means horizontal static gravity is approximately +Z. + -1.0 means horizontal static gravity is approximately -Z. + """ + roll = math.radians(roll_deg) + pitch = math.radians(pitch_deg) + + g_x = -math.sin(pitch) + g_y = math.cos(pitch) * math.sin(roll) + g_z = math.cos(pitch) * math.cos(roll) + + return np.array([g_x, g_y, g_z], dtype=float) * gravity_sign + + +def add_verification( + samples, + gravity_sign=1.0, + bias_seconds=1.0, + acc_deadband=0.10, + vel_decay=0.995, +): + """ + Add Provider-method gravity compensation and simple velocity integration. + + Output columns: + Provider_gx_g / Provider_gy_g / Provider_gz_g + Provider_lin_acc_x_g / y / z + Provider_lin_acc_x_ms2 / y / z + Provider_vel_x_ms / y / z + Provider_lin_acc_norm_ms2 + Provider_vel_norm_ms + """ + if not samples: + return samples + + raw_lin_acc_ms2 = [] + + for sample in samples: + acc_body_g = np.array( + [ + sample["acc_x_g"], + sample["acc_y_g"], + sample["acc_z_g"], + ], + dtype=float, + ) + + gravity_body_g = verify_gravity_body_g( + sample["angle_x_deg"], + sample["angle_y_deg"], + gravity_sign=gravity_sign, + ) + + linear_acc_body_g = acc_body_g - gravity_body_g + linear_acc_body_ms2 = linear_acc_body_g * 9.81 + + sample["Provider_gx_g"] = gravity_body_g[0] + sample["Provider_gy_g"] = gravity_body_g[1] + sample["Provider_gz_g"] = gravity_body_g[2] + + sample["Provider_lin_acc_x_g_raw"] = linear_acc_body_g[0] + sample["Provider_lin_acc_y_g_raw"] = linear_acc_body_g[1] + sample["Provider_lin_acc_z_g_raw"] = linear_acc_body_g[2] + + raw_lin_acc_ms2.append(linear_acc_body_ms2) + + raw_lin_acc_ms2 = np.array(raw_lin_acc_ms2, dtype=float) + + t0 = samples[0]["t_s"] + bias_indices = [ + i + for i, sample in enumerate(samples) + if sample["t_s"] - t0 <= bias_seconds + ] + + if bias_indices: + acc_bias = np.mean(raw_lin_acc_ms2[bias_indices], axis=0) + else: + acc_bias = np.zeros(3, dtype=float) + + print("Provider method") + print(f" gravity_sign: {gravity_sign}") + print(f" bias_seconds: {bias_seconds}") + print(f" estimated acc bias m/s^2: {acc_bias}") + + v = np.zeros(3, dtype=float) + last_t = samples[0]["t_s"] + + for i, sample in enumerate(samples): + t = sample["t_s"] + dt = t - last_t + last_t = t + + if dt <= 0 or dt > 0.2: + dt = 0.0 + + linear_acc = raw_lin_acc_ms2[i] - acc_bias + + if np.linalg.norm(linear_acc) < acc_deadband: + linear_acc[:] = 0.0 + + v = v * vel_decay + linear_acc * dt + + sample["Provider_lin_acc_x_ms2"] = linear_acc[0] + sample["Provider_lin_acc_y_ms2"] = linear_acc[1] + sample["Provider_lin_acc_z_ms2"] = linear_acc[2] + sample["Provider_lin_acc_norm_ms2"] = float(np.linalg.norm(linear_acc)) + + sample["Provider_vel_x_ms"] = v[0] + sample["Provider_vel_y_ms"] = v[1] + sample["Provider_vel_z_ms"] = v[2] + sample["Provider_vel_norm_ms"] = float(np.linalg.norm(v)) + + return samples + + +def unique_path(path): + if not path: + return path + + root, ext = os.path.splitext(os.path.basename(path)) + ext = ext.lower() + + if ext == ".csv": + folder = "csv" + elif ext == ".png": + folder = "png" + else: + folder = "output" + + os.makedirs(folder, exist_ok=True) + + new_path = os.path.join(folder, root + ext) + + if not os.path.exists(new_path): + return new_path + + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + return os.path.join(folder, f"{root}_{timestamp}{ext}") + + +def write_csv(samples, path): + if not samples: + return + + columns = list(samples[0].keys()) + + with open(path, "w", newline="", encoding="utf-8") as f: + writer = csv.DictWriter(f, fieldnames=columns) + writer.writeheader() + writer.writerows(samples) + + +def print_stats(samples): + if not samples: + print("No samples parsed.") + return + + print(f"Samples: {len(samples)}") + print(f"Duration: {samples[-1]['t_s'] - samples[0]['t_s']:.3f}s") + + groups = ( + ("RAW ACC g", ("acc_x_g", "acc_y_g", "acc_z_g")), + ("GYRO dps", ("gyro_x_dps", "gyro_y_dps", "gyro_z_dps")), + ("ANGLE deg", ("angle_x_deg", "angle_y_deg", "angle_z_deg")), + ( + "Provider GRAVITY g", + ("Provider_gx_g", "Provider_gy_g", "Provider_gz_g"), + ), + ( + "Provider LINEAR ACC m/s^2", + ( + "Provider_lin_acc_x_ms2", + "Provider_lin_acc_y_ms2", + "Provider_lin_acc_z_ms2", + "Provider_lin_acc_norm_ms2", + ), + ), + ( + "Provider VELOCITY m/s", + ( + "Provider_vel_x_ms", + "Provider_vel_y_ms", + "Provider_vel_z_ms", + "Provider_vel_norm_ms", + ), + ), + ) + + for group_name, fields in groups: + print(f"\n{group_name}") + for field in fields: + values = np.array([sample[field] for sample in samples], dtype=float) + diffs = np.diff(values) + + diff_std = float(np.std(diffs)) if len(diffs) else 0.0 + + print( + " {:24s} mean={: .6f} std={:.6f} ptp={:.6f} final={: .6f} diff_std={:.6f}".format( + field, + float(np.mean(values)), + float(np.std(values)), + float(np.ptp(values)), + float(values[-1]), + diff_std, + ) + ) + + freq = np.array([sample["freq_hz"] for sample in samples[1:]], dtype=float) + + if len(freq): + print( + "\nFrequency Hz mean={:.3f} std={:.3f} min={:.3f} max={:.3f}".format( + float(np.mean(freq)), + float(np.std(freq)), + float(np.min(freq)), + float(np.max(freq)), + ) + ) + + +def plot_group(axis, t, samples, fields, ylabel): + for field in fields: + values = np.array([sample[field] for sample in samples], dtype=float) + axis.plot(t, values, label=field, linewidth=1.0) + + axis.set_ylabel(ylabel) + axis.grid(True, alpha=0.3) + axis.legend(loc="upper right", ncol=3) + + +def plot_samples(samples, output=None, show=True): + if not samples: + raise ValueError("No samples to plot.") + + t = np.array([sample["t_s"] for sample in samples], dtype=float) + + fig, axes = plt.subplots(7, 1, sharex=True, figsize=(13, 15)) + + plot_group( + axes[0], + t, + samples, + ("acc_x_g", "acc_y_g", "acc_z_g"), + "Raw acc (g)", + ) + + plot_group( + axes[1], + t, + samples, + ("Provider_gx_g", "Provider_gy_g", "Provider_gz_g"), + "Estimated gravity (g)", + ) + + plot_group( + axes[2], + t, + samples, + ( + "Provider_lin_acc_x_ms2", + "Provider_lin_acc_y_ms2", + "Provider_lin_acc_z_ms2", + ), + "Provider linear acc (m/s²)", + ) + + plot_group( + axes[3], + t, + samples, + ("Provider_lin_acc_norm_ms2",), + "Linear acc norm (m/s²)", + ) + + plot_group( + axes[4], + t, + samples, + ("Provider_vel_x_ms", "Provider_vel_y_ms", "Provider_vel_z_ms"), + "Provider velocity (m/s)", + ) + + plot_group( + axes[5], + t, + samples, + ("gyro_x_dps", "gyro_y_dps", "gyro_z_dps"), + "Gyro (deg/s)", + ) + + plot_group( + axes[6], + t, + samples, + ("angle_x_deg", "angle_y_deg", "angle_z_deg"), + "Angle (deg)", + ) + + axes[6].set_xlabel("Time (s)") + + fig.suptitle("Provider Method IMU Verification") + fig.tight_layout() + + if output: + fig.savefig(output, dpi=160) + print(f"Saved plot to {output}") + + if show: + plt.show() + + +def main(): + parser = argparse.ArgumentParser( + description="Verify Provider roll/pitch gravity compensation method." + ) + + source = parser.add_mutually_exclusive_group(required=True) + source.add_argument("--file", help="pc_reader text log, for example rcd.txt") + source.add_argument("--port", help="ESP32 serial port, for example /dev/ttyUSB0") + + parser.add_argument("--baud", type=int, default=115200) + parser.add_argument("--seconds", type=float, default=10.0) + parser.add_argument("--samples", type=int) + parser.add_argument("--max-gap-ms", type=int, default=1000) + + parser.add_argument( + "--gravity-sign", + type=float, + default=1.0, + choices=[1.0, -1.0], + help="Use +1 if horizontal static acc_z is about +1g; use -1 if about -1g.", + ) + + parser.add_argument( + "--bias-seconds", + type=float, + default=1.0, + help="Use the first N seconds to estimate residual linear acceleration bias.", + ) + + parser.add_argument( + "--acc-deadband", + type=float, + default=0.10, + help="Set linear acceleration to zero if norm is below this value, unit m/s^2.", + ) + + parser.add_argument( + "--vel-decay", + type=float, + default=0.995, + help="Velocity decay factor to reduce long-term integration drift.", + ) + + parser.add_argument("--output", default="verify_l.png") + parser.add_argument("--csv", default="verify_l.csv") + parser.add_argument("--no-show", action="store_true") + + args = parser.parse_args() + + if args.file: + samples = read_text_log(args.file) + else: + samples = read_serial_samples( + args.port, + args.baud, + args.seconds, + args.samples, + args.max_gap_ms, + ) + + if not samples: + print("No samples parsed. Check input mode, serial port, baudrate, or log format.") + return + + samples = add_verification( + samples, + gravity_sign=args.gravity_sign, + bias_seconds=args.bias_seconds, + acc_deadband=args.acc_deadband, + vel_decay=args.vel_decay, + ) + + print_stats(samples) + + csv_path = unique_path(args.csv) + write_csv(samples, csv_path) + print(f"Saved CSV to {csv_path}") + + output_path = unique_path(args.output) + plot_samples(samples, output=output_path, show=not args.no_show) + + +if __name__ == "__main__": + main() +# python3 verify.py --port /dev/ttyUSB0 --baud 115200 --seconds 10 --no-show \ No newline at end of file