# IMU 惯性三维里程计 + 3D 坐标显示

## 概述

基于 STM32 IMU 串口解码系统，在 PC 端实现 3D 轨迹重建和实时可视化。下位机 (STM32) 已完成 EKF 姿态估计，PC 端接收四元数姿态和滤波后加速度数据，进行偏置标定、重力补偿与双重积分，重建三维运动轨迹。

## 架构

```
STM32 (200Hz)
  └── 串口帧 (48 bytes): header + timestamp(uint32) + gyro[3] + accel[3] + quat[4] + CRC16
        │
PC 端管线:
  ├── imu_decode.py          # 串口帧解析 (CRC 校验 + 解包 48 字节帧)
  ├── trajectory_tracker.py  # 核心算法 (四元数→旋转, 偏置标定, 重力补偿, 双重积分, ZUPT)
  ├── visualize_3d.py        # matplotlib 3D 动画窗口
  └── main_odometry.py       # 主入口 (串联所有模块)
```

## 串口协议 (v2)

| 偏移 | 大小 | 内容 |
|------|------|------|
| 0 | 2B | 帧头 0xAA 0x55 |
| 2 | 4B | 时间戳 uint32_t (ms, 自启动) |
| 6 | 12B | 滤波后 Gyro[3] (float32 × 3) |
| 18 | 12B | 滤波后 Accel[3] (float32 × 3) |
| 30 | 16B | 四元数 qw, qx, qy, qz (float32 × 4) |
| 46 | 2B | CRC16 (对前 46 字节) |

**坐标系**: 右手系, X-前 Y-左 Z-上 (Z-up)

## 算法管线

```
串口帧 (已解析)
  ├── timestamp ms ──→ dt = Δts / 1000 (精确时间步长)
  ├── gyro[3] rad/s ──→ 减 gyro_bias → ZUPT 静止检测
  ├── accel[3] m/s^2 ──→ 减 accel_bias (EKF 四元数标定)
  └── qw/qx/qy/qz ──→ 四元数 → 旋转矩阵 (scipy Rotation.from_quat)
                │
        a_world = R @ (a_body - accel_bias)
        a_linear = a_world - [0, 0, 9.81]
                │
        加速度死区 (|a_i| < 0.03 → 0)
        ZUPT: ‖gyro‖ < 0.05 AND ‖a_linear‖ < 0.20 AND var(a_linear) < 0.005
                │
        梯形积分 → 速度 → 位置 (使用时间戳真实 dt)
                │
        matplotlib 3D 实时轨迹显示
```

### 关键: 加速度计偏置标定

EKF 四元数与加速度计之间存在不一致时会导致积分漂移。启动时采集 200 帧静止数据，利用 EKF 四元数标定：

```
R = from_quat(q_mean)
gravity_body_expected = R^T @ [0, 0, 9.81]
accel_bias = mean(accel_measured) - gravity_body_expected
```

标定后 `R @ (accel - bias) ≈ [0, 0, 9.81]`，静止时 `a_linear ≈ 0`，ZUPT 正常触发。

## 依赖

```bash
pip install numpy matplotlib pyserial scipy
```

## 用法

### 实时模式

```bash
python main_odometry.py COM5
python main_odometry.py COM5 921600
python main_odometry.py COM5 --save traj.csv
```

### 回放模式

```bash
python main_odometry.py --replay traj.csv
```

回放时按 **空格键** 暂停/继续。

### 调试

```bash
python imu_decode.py COM5          # 仅解析帧并打印
python test_3d_demo.py             # 模拟数据 3D 演示(无需串口)
```

## 文件说明

### `imu_decode.py`
串口帧解析模块。48 字节帧 = 2B header + 44B payload + 2B CRC16。

- `parse_frame(payload)` → dict: timestamp_ms, gyro(3), accel(3), quat(4)
- `quat_to_euler(qw,qx,qy,qz)` → yaw, pitch, roll (deg)

### `trajectory_tracker.py`
核心跟踪算法：

- `quat_to_rotation(qw,qx,qy,qz)` — 四元数 → 旋转矩阵
- `rotate_accel(accel_body, R)` — 机体→世界坐标
- `gravity_compensate(a_world)` — 减重力 [0, 0, 9.81]
- `apply_deadzone(a)` — 加速度死区
- `Tracker` — 位置/速度/姿态跟踪器
- `Tracker.calibrate_from_samples()` — 利用四元数标定偏置

| ZUPT 参数 | 默认值 |
|-----------|--------|
| `zupt_threshold_accel` | 0.20 m/s^2 |
| `zupt_threshold_gyro` | 0.05 rad/s |
| `zupt_frames` | 15 帧 |
| `deadzone_threshold` | 0.03 m/s^2 |
| `var_window_size` | 30 帧 |
| `zupt_var_threshold` | 0.005 m^2/s^4 |

### `visualize_3d.py`
matplotlib 3D 实时显示：蓝色轨迹线、红点当前位置、原点 RGB 坐标轴、自适应等比例坐标。

### `main_odometry.py`
运行入口：串口采集 + 3D 显示、CSV 保存、回放。dt 由时间戳差值计算。

## CSV 格式

```csv
timestamp_ms,gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z,qw,qx,qy,qz,pos_x,pos_y,pos_z
12345,0.001,0.002,-0.001,0.75,-0.05,9.81,0.996,0.001,-0.002,-0.036,0.000,0.000,0.000
...
```

## 验证结果

| 场景 | 结果 |
|------|------|
| 25s 静止 (偏置标定后) | 0.000m 漂移 |
| 2 m/s^2 × 0.5s 运动 | 0.250m 位移 (理论值) |

## 调试

- 漂移：检查标定输出 `accel_bias` 是否合理 (通常 X/Y < 0.8, Z < 0.1)
- ZUPT 不触发：增大 `zupt_threshold_accel` 或减小 `zupt_var_threshold`
- 3D 卡顿：增大 `refresh_interval`