ShunFang-cart/ESP
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add verification script for IMU gravity compensation and velocity integration

Browse Source
This commit is contained in:
Brunsmeier
2026-07-02 11:19:53 +08:00
parent e0f777837b
commit be7c498270
2 changed files with 624 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
README.md
View File

@ -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`: