# Research: SLAM, IMU, and Sensor Fusion for TurboPi Robot Car

## Context

Session 59 (2026-04-11). Investigated SLAM algorithms, IMU options, and
sensor agreement for the TurboPi robot car's autonomous navigation.

## Sensor Comparison: Sonar vs Lidar

### Test Setup

Both sensors measured the same forward obstacle simultaneously.
5 paired readings taken via `/distance` and `/scan` endpoints.

### Results

| Sensor | Distance | Variance | Position |
|--------|----------|----------|----------|
| Sonar (HC-SR04 ultrasonic) | 749 mm | 0 mm | Front face of chassis |
| Lidar Forward sector (RPLIDAR C1) | ~877 mm | ±0.5 mm | Center of chassis |
| **Delta** | **128 mm** | **consistent** | **= mounting offset** |

**Conclusion:** Sensors agree. The 128mm delta is the physical distance
between their mounting positions. Safety daemon should account for this
when fusing sonar + lidar distances.

### Lidar Self-Reflections

| Sector | Distance | Label | Cause |
|--------|----------|-------|-------|
| Right (sector 3) | 37.6 mm | blocked | Chassis/bracket reflection |
| Right-Rear (sector 4) | 33.8 mm | blocked | Chassis/bracket reflection |

These are NOT real obstacles. Should be filtered to prevent false ESTOP triggers.

### Lidar Calibration Status

- `LIDAR_FORWARD_OFFSET_DEG` = 0.0 (uncalibrated)
- Sector 0 ("Forward") may not align perfectly with camera/sonar forward
- Calibration procedure: place object at camera center, read lidar angle, set offset

## SLAM Algorithm Selection

### Candidates Evaluated

| Library | Verdict | Reason |
|---------|---------|--------|
| **HectorSLAM (numpy port)** | **CHOSEN** | Lidar-only scan matching, no odom needed, ~300 LOC |
| BreezySLAM | Rejected | Python C extension has Py3.13 breakage risk |
| GMapping | Rejected | Requires wheel encoder odometry (TurboPi has none) |
| Cartographer | Rejected | ROS2 + heavy deps, overkill for apartment |
| Custom ICP only | Rejected | No multi-resolution → local minima traps |

### Why HectorSLAM

HectorSLAM (Kohlbrecher et al., 2011) uses Gauss-Newton scan-to-map matching
on multi-resolution occupancy grids. Key advantages for TurboPi:

1. **No wheel encoders needed** — uses lidar scan matching for pose estimation
2. **IMU optional but helpful** — gyro provides yaw prior, scan matcher corrects drift
3. **Lightweight** — core algorithm is ~300 lines of linear algebra (numpy)
4. **Multi-resolution** — coarse-to-fine matching avoids local minima
5. **No ROS dependency** — we port the core algorithm to Python/numpy

### Algorithm Overview

```
For each lidar scan:
1. PREDICT:  pose_estimate = last_pose + IMU_yaw_delta
2. MATCH:    for level in [coarse, medium, fine]:
               pose = gauss_newton_match(scan, grid[level], pose)
3. UPDATE:   ray_cast(scan, pose) → update finest grid
4. PUBLISH:  (x, y, θ) + occupancy_grid
```

## IMU Selection: MPU-6050

### HiWonder Board IMU Status

The HiWonder ROS Controller board SDK has `board.get_imu()` (PACKET_FUNC_IMU = 7,
returns `ax, ay, az, gx, gy, gz` as 6 floats). **Tested: returns None for all
readings.** The TurboPi variant's STM32 never sends IMU data. No IMU chip on board.

### MPU-6050 Specs

| Spec | Value |
|------|-------|
| Axes | 3-axis gyro + 3-axis accelerometer |
| I2C address | 0x68 (AD0 low) |
| Gyro range | ±250/500/1000/2000 °/s |
| Gyro sensitivity | 131 LSB/(°/s) at ±250°/s |
| Supply voltage | 2.375V - 3.46V (chip), 5V OK on breakout modules |
| Current draw | ~3.5 mA active |
| Gyro drift | ~1-3°/min (typical, mitigated by scan matcher) |

### Connection: Pi Pico RP2040 USB Bridge

**Why not direct I2C to Pi 5:** The HiWonder hat covers the Pi 5 GPIO header
completely. No direct pin access. Hat provides only SDA, SCL, GND, 5V breakout.

**Solution:** Pi Pico RP2040 as a dedicated IMU processor:
- Pico reads MPU-6050 via its own I2C0 (separate from Pi's bus)
- Sends processed `heading_deg,gyro_z_dps` over USB serial at 100 Hz
- Pi 5 reads `/dev/ttyACM0` as simple serial stream

### Verified Wiring (WORKING)

```
MPU-6050        Pi Pico RP2040
────────        ──────────────
VCC  ────────── 3V3 (Pin 36, below 3V3_EN)
GND  ────────── GND (Pin 38, above 3V3_EN)
SDA  ────────── GP4 (Pin 6)  — I2C0 SDA alternate
SCL  ────────── GP5 (Pin 7)  — I2C0 SCL alternate
```

Pico → Pi 5 via USB cable (micro-USB to USB-A).

### MPU-6050 Mounting

- **Must be flat (horizontal)** — gyro Z-axis perpendicular to board = yaw
- Rotation orientation doesn't matter (only angular rate changes used)
- Mounted on bottom of chassis (battery area) with foam tape
- A few cm from RPLIDAR motor (vibration concern, not EMI)
- Center of chassis preferred for minimal centripetal acceleration on accelerometer
  (but we only use gyro-Z, so position doesn't affect readings)

### I2C Gotcha

I2C at 400 kHz caused `ETIMEDOUT` during calibration (200 rapid reads).
**Fix: 100 kHz** — jumper wire capacitance slows signal edges. 100 kHz is
the safe speed for breadboard-style wiring.

### Firmware

MicroPython v1.24.0 on Pico. Key details:
- `I2C(0, sda=Pin(4), scl=Pin(5), freq=100_000)`
- Zero-rate calibration on boot: 100 samples at 20ms intervals (~2s)
- Error handling: skips occasional I2C timeouts, exits after 50 consecutive failures
- Deploy via `mpremote`: `~/.local/bin/mpremote connect /dev/ttyACM0 cp main.py :main.py + reset`

### Verified Output

```
heading_deg, gyro_z_dps
0.02,       -0.09       ← stationary
169.65,      0.34       ← after rotation
263.28,      0.37       ← after further rotation
```

Drift observed: ~4°/min. Normal for MPU-6050. Corrected by SLAM scan matcher.

## I2C Address Map (Full Robot)

| Bus | Address | Device | Status |
|-----|---------|--------|--------|
| Pi 5 I2C1 | 0x77 | Sonar (ultrasonic + RGB LEDs) | Active, LEDs off |
| Pi 5 I2C1 | 0x78? | Line follower (4-channel) | Present |
| Pico I2C0 | 0x68 | MPU-6050 (gyro + accel) | Active, verified |

No bus conflicts — Pico has its own independent I2C bus.

## GPIO Usage (HiWonder Hat)

The hat uses only 6 of 40 GPIO pins:

| Pin | GPIO | Used For |
|-----|------|----------|
| 2, 4 | 5V | Board power (STM32 + motors) |
| 3 | GPIO 2 (SDA) | I2C bus 1 |
| 5 | GPIO 3 (SCL) | I2C bus 1 |
| 8 | GPIO 14 (TX) | UART to STM32 (/dev/ttyAMA0) |
| 10 | GPIO 15 (RX) | UART from STM32 |
| 6,9,14 | GND | Common ground |

All other 30+ pins pass through but are physically inaccessible under the hat.

## Next Steps

1. Create udev rule for `/dev/imu` symlink (VID 2e8a, PID 0005)
2. Implement `imu.py` serial reader on Pi 5
3. Implement HectorSLAM core (`slam.py`) — numpy port
4. Add `/map` and `/pose` endpoints
5. Upgrade Annie's navigation loop with map-aware pathfinding
6. Filter lidar self-reflections (Right/Right-Rear sectors)
7. Calibrate `LIDAR_FORWARD_OFFSET_DEG`

See `docs/NEXT-SESSION-HECTORSLAM-IMU.md` for the full implementation plan.