# Next Session: HectorSLAM + MPU-6050 IMU

## TL;DR

Port HectorSLAM's core algorithm to Python/numpy for the TurboPi robot car.
IMU hardware is DONE — MPU-6050 streams heading via Pi Pico USB bridge at 100 Hz.
**This session: implement SLAM core (Phase 1), server endpoints (Phase 2),
Annie navigation upgrade (Phase 3), and calibrate on hardware (Phase 4).**

## Status

| Phase | Status | What |
|-------|--------|------|
| **Phase 0** | **DONE** (session 59) | Pico flashed, MPU-6050 wired, heading verified (0°→263°→170°) |
| Phase 1 | TODO | HectorSLAM core (`slam.py`) — numpy port |
| Phase 2 | TODO | Server integration (`/map`, `/pose` endpoints) |
| Phase 3 | TODO | Annie navigation upgrade (map-aware pathfinding) |
| Phase 4 | TODO | Live calibration + testing |

## What's Already Working

- **MPU-6050 → Pico RP2040 → Pi 5 USB serial** — verified, streaming `heading_deg,gyro_z_dps` at 100 Hz
- **Pico firmware** (`main.py`): MicroPython v1.24.0, auto-calibrates on boot, error handling for I2C timeouts
- **Pico appears as** `/dev/ttyACM0` on Pi 5 (udev rule for `/dev/imu` not yet created)
- **RPLIDAR C1** — already running via LidarDaemon, 12-sector summaries, DenseBoost mode
- **Safety daemon** — Hailo YOLO + sonar + lidar fusion, unchanged by SLAM work

## Background

Current navigation is **reactive** — lidar gives 12 sectors ("Forward: clear
3.2m, Right: blocked 0.4m"), VLM picks a direction, car drives 1 second.
No memory of where it's been, no map, no localization. Annie can explore
a room but can't navigate *to* a specific location or avoid revisiting areas.

**HectorSLAM** (Kohlbrecher et al., 2011) is ideal because:
- **Lidar-only** scan matching — no wheel encoders needed (TurboPi has none)
- Uses Gauss-Newton optimization on multi-resolution occupancy grids
- Lightweight: core algorithm is ~300 lines of C++
- IMU optional but significantly improves yaw estimation during fast turns

**MPU-6050** provides 3-axis gyroscope (yaw rate) + 3-axis accelerometer.
Connected via Pi Pico RP2040 bridge (GP4=SDA, GP5=SCL, 100kHz I2C).
Heading stream verified on Pi 5 at `/dev/ttyACM0`.

## Research References

- `docs/RESEARCH-SLAM-IMU-SENSORS.md` — full research: algorithm comparison, wiring, sensor data
- `docs/RESEARCH-TURBOPI-CAPABILITIES.md` — hardware inventory, capability gaps, MentorPi comparison

## Hardware Setup (Phase 0)

### Approach: Pi Pico RP2040 as USB-Serial IMU Bridge

The HiWonder hat covers the Pi 5 GPIO header completely — no direct pin
access. Instead of using the hat's limited breakout, we use a **Pi Pico
RP2040** as a dedicated IMU processor. The Pico reads the MPU-6050 via its
own I2C bus and sends processed heading data to the Pi 5 over USB serial.

**Advantages over direct I2C:**
- Clean 3.3V from Pico's onboard regulator (no 5V risk)
- Separate I2C bus (no contention with sonar/line follower)
- Dedicated processor handles 100 Hz polling + calibration
- No dependency on HiWonder hat breakout
- USB cable = simple, reliable connection

### MPU-6050 → Pico Wiring

```
MPU-6050 Module     Pi Pico RP2040
───────────────     ──────────────
VCC  ────────────── 3V3 (Pin 36)   ← clean regulated 3.3V (below 3V3_EN)
GND  ────────────── GND (Pin 38)   ← above 3V3_EN
SDA  ────────────── GP4 (Pin 6)    ← I2C0 SDA alternate
SCL  ────────────── GP5 (Pin 7)    ← I2C0 SCL alternate
AD0  ────────────── GND or float   (address 0x68)
INT  ────────────── not connected
```

### Pico → Pi 5 Connection

Connect Pico to Pi 5 via **USB cable** (micro-USB on Pico to any USB-A on Pi 5).
The Pico appears as `/dev/ttyACM0` on the Pi 5.

### Flash MicroPython on Pico

```bash
# 1. Hold BOOTSEL button on Pico, plug USB into your computer
# 2. Pico mounts as USB drive (RPI-RP2)
# 3. Download MicroPython UF2 from:
#    https://micropython.org/download/RPI_PICO/
# 4. Drag the .uf2 file onto RPI-RP2 drive
# 5. Pico reboots with MicroPython installed
```

### Pico Firmware (main.py)

Save this as `main.py` on the Pico. It auto-runs on boot.

```python
# main.py — Pi Pico RP2040 IMU bridge for HectorSLAM
# Reads MPU-6050 gyro-Z at 100 Hz, integrates heading, sends over USB serial.
from machine import I2C, Pin
import struct, time, sys

i2c = I2C(0, sda=Pin(4), scl=Pin(5), freq=100_000)  # 100kHz for jumper wires
MPU = 0x68

# Wake up MPU-6050 (clear sleep bit in PWR_MGMT_1)
i2c.writeto_mem(MPU, 0x6B, b'\x00')
time.sleep_ms(100)

# Verify WHO_AM_I
who = i2c.readfrom_mem(MPU, 0x75, 1)[0]
print(f"MPU6050 WHO_AM_I: 0x{who:02x}")

# Zero-rate calibration: average 200 gyro-Z readings while stationary (~2s)
print("Calibrating gyro (hold still)...")
cal_sum = 0.0
for _ in range(200):
    raw = i2c.readfrom_mem(MPU, 0x47, 2)  # GYRO_ZOUT_H/L
    cal_sum += struct.unpack('>h', raw)[0]
    time.sleep_ms(5)
gz_offset = cal_sum / 200.0
print(f"Calibration done. Offset: {gz_offset:.1f}")

# Main loop: integrate gyro-Z for heading
heading = 0.0
last_us = time.ticks_us()

while True:
    raw = i2c.readfrom_mem(MPU, 0x47, 2)
    gz_raw = struct.unpack('>h', raw)[0] - gz_offset
    gz_dps = gz_raw / 131.0  # LSB/(deg/s) for +/-250 deg/s range

    now_us = time.ticks_us()
    dt = time.ticks_diff(now_us, last_us) / 1_000_000.0
    last_us = now_us

    heading = (heading + gz_dps * dt) % 360.0

    # Output: heading_deg,gyro_z_dps
    sys.stdout.write(f"{heading:.2f},{gz_dps:.2f}\n")

    time.sleep_ms(10)  # 100 Hz
```

### Verify on Pi 5

```bash
# After Pico is flashed and connected via USB:

# 1. Check Pico shows up as serial device:
ls /dev/ttyACM*
# Should show: /dev/ttyACM0

# 2. Read IMU data stream:
cat /dev/ttyACM0
# Should print lines like: 0.15,-0.32
#                           0.28,0.14
#                           ...

# 3. Quick Python test:
python3 -c "
import serial, time
s = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
time.sleep(2)  # Wait for Pico calibration
for _ in range(10):
    line = s.readline().decode().strip()
    if ',' in line:
        heading, gz = line.split(',')
        print(f'Heading: {heading} deg, Gyro-Z: {gz} deg/s')
s.close()
"
```

### udev Rule for Stable Device Name

```bash
# On Pi 5, create /etc/udev/rules.d/99-turbopi.rules:
# (add this line alongside existing lidar rule)
SUBSYSTEM=="tty", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="0005", SYMLINK+="imu"
# Then: sudo udevadm control --reload-rules && sudo udevadm trigger
# Pico will appear as /dev/imu (stable across reboots)
```

## Architecture

```
Pi 5:
  USB serial (/dev/imu) ←── Pi Pico RP2040 ←── MPU-6050 (I2C0)
       │                      (MicroPython)      (0x68, 3.3V)
       │                      100 Hz heading
       ▼
  IMU Reader (serial, 100 Hz)  ─┐
  LidarDaemon (RPLIDAR C1)     ├── SlamDaemon ── FastAPI :8080 ── Annie (Titan)
  FrameGrabber (camera)        ─┤       │                            │
  HailoSafetyDaemon (YOLO)    ─┘       │                            ▼
       │                                ▼                     Gemma 4 26B vLLM
       │                       Occupancy Grid Map              image + map + pose
       │                       + Robot Pose (x,y,θ)            → navigation action
       └──── ESTOP (unchanged, still overrides everything)
```

### New Components

| Component | File | What |
|-----------|------|------|
| Pico firmware | `pico/main.py` | MicroPython: MPU-6050 read + heading integration |
| IMU reader | `imu.py` | Serial reader (/dev/imu), parses Pico output |
| SLAM daemon | `slam.py` | HectorSLAM core: scan matching + mapping |
| Map endpoint | `main.py` | `GET /map` (occupancy grid PNG), `GET /pose` |
| Nav upgrade | `robot_tools.py` | Query map/pose, pathfinding, goal-directed nav |

### What Doesn't Change

- Safety daemon (Hailo YOLO + sonar + lidar ESTOP) — unchanged
- Lidar daemon — SlamDaemon reads raw points via new accessor
- FrameGrabber — unchanged
- Existing endpoints — unchanged

## HectorSLAM Algorithm (Core)

### The Idea

Maintain a **multi-resolution occupancy grid** (3 levels: 5cm, 10cm, 20cm).
For each new lidar scan:

```
1. PREDICT: pose_estimate = last_pose + IMU yaw delta
2. MATCH:   for each resolution (coarse → fine):
               pose_estimate = gauss_newton_match(scan, grid[level], pose_estimate)
3. UPDATE:  ray_cast(scan, pose_estimate) → update finest grid
4. PUBLISH: pose = pose_estimate, map = grid[0]
```

### Gauss-Newton Scan Matching

For each scan point `p_i` in the lidar scan:
1. Transform to world coordinates: `w_i = rotate(p_i, θ) + (x, y)`
2. Look up occupancy value `M(w_i)` via bilinear interpolation on the grid
3. Compute gradient: `∂M/∂x`, `∂M/∂y` (central differences on grid)
4. Build 3x3 Hessian `H` and 3x1 gradient vector `b`:
   ```
   J_i = [∂M/∂x, ∂M/∂y, ∂M/∂θ]  (1x3 Jacobian per point)
   H += J_i^T * J_i
   b += J_i^T * (1.0 - M(w_i))
   ```
5. Solve: `Δξ = H^{-1} * b` where `ξ = (Δx, Δy, Δθ)`
6. Update: `pose += Δξ`
7. Repeat 3-5 times (typically converges in 2-3 iterations)

### Multi-Resolution Strategy

| Level | Cell Size | Grid Size (10m×10m) | Purpose |
|-------|-----------|---------------------|---------|
| 0 (fine) | 5 cm | 200×200 | Final matching + map storage |
| 1 (medium) | 10 cm | 100×100 | Intermediate refinement |
| 2 (coarse) | 20 cm | 50×50 | Rough alignment, avoids local minima |

Coarse levels are downsampled from fine level (max-pool occupancy).

### Occupancy Grid Update

After pose is matched, update the map via ray casting:
- For each scan point: draw a ray from robot to point using Bresenham
- Cells along the ray → decrease occupancy (free space)
- Cell at the endpoint → increase occupancy (obstacle)
- Use log-odds representation: `logodds += hit_increment` or `logodds -= miss_decrement`
- Clamp to [-5, 5] to prevent over-confidence

### IMU Integration (via Pico USB bridge)

The Pi Pico handles all MPU-6050 I2C reads and gyro integration. It sends
pre-computed `heading_deg,gyro_z_dps` lines over USB serial at 100 Hz.
The Pi 5 just reads the serial stream — no I2C, no raw register access.

**Pico-side (MicroPython):**
- Zero-rate calibration at boot (200 samples, ~2s)
- Gyro-Z integration at 100 Hz: `heading += gz_dps * dt`
- Output: `"{heading:.2f},{gz_dps:.2f}\n"` over USB serial

**Pi 5-side (`imu.py`):**
- Read `/dev/imu` (udev symlink for Pico) at 115200 baud
- Parse heading and gyro values from each line
- Thread-safe `get_heading_deg()` and `get_heading_delta_deg()`

**Gyro drift mitigation:**
- Zero-rate offset calibration on Pico boot (average 200 readings while stationary)
- The scan matcher corrects residual drift — IMU is just the initial guess
- Do NOT use accelerometer for heading (no magnetometer = no absolute reference)

## Implementation Plan

### Phase 0: Hardware + Pico Firmware — DONE (session 59)

All hardware verified working:
- [x] MPU-6050 wired to Pico (3V3→pin36, GND→pin38, SDA→GP4, SCL→GP5)
- [x] MicroPython v1.24.0 flashed on Pico
- [x] `main.py` firmware deployed (100Hz heading, auto-calibration, error handling)
- [x] I2C at 100kHz (400kHz caused ETIMEDOUT with jumper wires)
- [x] Heading verified: 0° → 263° → 170° (tracks rotation correctly)
- [x] Pico appears as `/dev/ttyACM0` on Pi 5
- [ ] udev rule for `/dev/imu` symlink (TODO — VID 2e8a, PID 0005)
- [ ] `imu.py` serial reader on Pi 5 (TODO — Phase 1 below)

**Deploy firmware:** `~/.local/bin/mpremote connect /dev/ttyACM0 cp main.py :main.py + reset`
**Verify:** `ssh pi "timeout 3 cat /dev/ttyACM0"` → should show `heading,gyro_z` lines

### Phase 1: IMU Reader + HectorSLAM Core (~2.5 hours)

**START HERE in the next session.**

1. **Create udev rule** for `/dev/imu` symlink:
   ```bash
   # /etc/udev/rules.d/99-turbopi.rules (add alongside lidar rule):
   SUBSYSTEM=="tty", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="0005", SYMLINK+="imu"
   ```
2. **Create `services/turbopi-server/imu.py`:**
   - `ImuReader(threading.Thread)` — reads serial `/dev/imu` at 115200
   - Parses `heading_deg,gyro_z_dps` from each line
   - Thread-safe `get_heading_deg() → float`
   - `get_heading_delta_deg() → float` — change since last call
   - `is_healthy() → bool` — True if recent valid line received
3. **Create `services/turbopi-server/test_imu.py`:**
   - Mock serial, test line parsing, heading delta, stale detection

4. **Create `services/turbopi-server/slam.py`:**
   - `OccupancyGrid` class — log-odds grid, bilinear interpolation, gradient
   - `MultiResGrid` — 3 levels, downsample from fine
   - `scan_match(points, grid, pose_estimate, max_iter=5) → pose`
   - `update_map(points, pose)` — Bresenham ray casting
   - `SlamDaemon(threading.Thread)`:
     - Consumes raw lidar points from LidarDaemon (new accessor)
     - Consumes heading from ImuDaemon
     - Runs predict→match→update loop at ~10 Hz
     - Thread-safe `get_pose()` and `get_map()` accessors
5. **Modify `services/turbopi-server/lidar.py`:**
   - Add `get_raw_points() → list[tuple[float, float]]` — returns latest
     raw (angle_deg, distance_mm) tuples for SLAM consumption
   - Keep existing `get_sectors()` for safety daemon (unchanged)
6. **Create `services/turbopi-server/test_slam.py`:**
   - Occupancy grid: update, bilinear interpolation, gradient
   - Scan matching: known-offset convergence test (place scan at 10cm offset,
     verify matcher recovers true pose within 1cm)
   - Ray casting: verify free/occupied cells along known ray
   - Multi-resolution: verify coarse-to-fine convergence

### Phase 2: Server Integration (~30 min)

7. **Modify `services/turbopi-server/main.py`:**
   - Wire ImuReader + SlamDaemon into lifespan (after lidar, before safety)
   - Add `GET /map` — returns occupancy grid as PNG image (or JSON)
   - Add `GET /pose` — returns `{x_m, y_m, heading_deg, confidence}`
   - Add `imu_healthy` and `slam_healthy` to `/health`
8. **Create `services/turbopi-server/test_slam_endpoints.py`:**
   - Mock slam daemon, test /map and /pose responses

### Phase 3: Annie Navigation Upgrade (~1 hour)

9. **Modify `services/annie-voice/robot_tools.py`:**
    - `handle_navigate_robot` fetches `/map` + `/pose` alongside photo/obstacles/scan
    - VLM prompt includes: "You are at position (x, y) facing θ°. Here is the
      map of explored areas (white=free, black=wall, gray=unknown)."
    - Add simple A* pathfinding on the occupancy grid for goal-directed nav
    - Navigation history becomes pose history (not just action list)
10. **Modify `services/annie-voice/tests/test_robot_tools.py`:**
    - Tests for map-aware navigation prompt, A* pathfinding

### Phase 4: Testing + Calibration (~30 min)

11. **Live calibration:**
    - Drive robot in a small square, verify map looks reasonable
    - Adjust map resolution if needed (5cm might be too fine for RPLIDAR C1)
    - Tune Gauss-Newton convergence (max_iter, damping)
    - Check CPU usage stays under 70%
12. **Integration test:**
    - "Annie, explore this room" → watch map build up
    - "Annie, go back to where you started" → uses pose history + map
    - Verify ESTOP still works during SLAM operation

## Key Design Decisions

1. **Pure numpy, no ROS** — Avoids ROS2 dependency. HectorSLAM's core is
   just linear algebra (matrix multiply, solve 3x3 system, bilinear interp).
   numpy handles all of this efficiently.

2. **SlamDaemon is separate from LidarDaemon** — Lidar daemon handles hardware
   (serial, reconnect, stall detection). SLAM daemon handles math (matching,
   mapping). Clean separation of concerns.

3. **IMU is a prior, not a requirement** — If MPU-6050 disconnects or isn't
   wired, SLAM falls back to lidar-only matching (like original HectorSLAM).
   Heading delta defaults to 0 (assume small rotation between scans).

4. **Map size: 10m × 10m at 5cm resolution** — 200×200 grid = 40KB.
   Apartment-scale. Can be expanded if needed (20m = 400×400 = 160KB).

5. **No loop closure** — HectorSLAM doesn't do loop closure (that's
   Cartographer/ORB-SLAM territory). For apartment exploration this is fine.
   Drift over long runs is mitigated by IMU + multi-resolution matching.

6. **Map as PNG** — Occupancy grid renders as a grayscale image:
   white=free (probability < 0.3), black=occupied (> 0.7), gray=unknown (0.5).
   Robot position drawn as a red arrow. Sent to VLM as an image.

## MPU-6050 Register Reference

| Register | Address | What |
|----------|---------|------|
| WHO_AM_I | 0x75 | Should return 0x68 |
| PWR_MGMT_1 | 0x6B | Write 0x00 to wake from sleep |
| GYRO_CONFIG | 0x1B | 0x00=±250°/s, 0x08=±500°/s |
| ACCEL_CONFIG | 0x1C | 0x00=±2g |
| ACCEL_XOUT_H | 0x3B | Start of 14-byte burst read |
| GYRO_XOUT_H | 0x43 | Gyro X high byte |
| GYRO_ZOUT_H | 0x47 | Gyro Z (yaw) high byte |

Burst read 14 bytes from 0x3B: `ax(2) ay(2) az(2) temp(2) gx(2) gy(2) gz(2)`
All values are signed 16-bit big-endian.

## Known Gotchas

| Gotcha | Mitigation |
|--------|------------|
| MPU-6050 gyro drift (~1-3°/min) | Zero-rate calibration at boot + scan matcher corrects |
| Mecanum wheel slip = bad odometry | We DON'T use wheel odometry. IMU gyro only. |
| RPLIDAR C1 scan rate ~10 Hz | HectorSLAM works at 10 Hz. Each rotation = one SLAM update. |
| Bilinear interpolation at grid edges | Clamp to grid bounds, return 0.5 (unknown) for out-of-bounds |
| Gauss-Newton divergence | Damping factor (lambda=0.01), cap Δξ to max step size |
| Pi 5 CPU budget | SLAM adds ~10-15% CPU. Total: safety(15%) + lidar(5%) + SLAM(15%) = ~35%. Safe. |
| I2C bus contention (sonar + IMU) | Different addresses (0x77 vs 0x68). smbus2 handles bus arbitration. |
| Map memory growth | Fixed 200×200 grid (40KB). No dynamic expansion. |

## SLAM Research Summary

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

## Files Summary

| File | Action | What |
|------|--------|------|
| `/etc/udev/rules.d/99-turbopi.rules` | MODIFY | Add `/dev/imu` symlink for Pico |
| `services/turbopi-server/imu.py` | CREATE | Serial reader for Pico IMU bridge |
| `services/turbopi-server/slam.py` | CREATE | HectorSLAM core + daemon |
| `services/turbopi-server/lidar.py` | MODIFY | Add `get_raw_points()` |
| `services/turbopi-server/main.py` | MODIFY | Wire ImuReader + SlamDaemon, add /map, /pose |
| `services/turbopi-server/test_imu.py` | CREATE | IMU serial reader tests |
| `services/turbopi-server/test_slam.py` | CREATE | SLAM unit tests |
| `services/turbopi-server/test_slam_endpoints.py` | CREATE | Endpoint tests |
| `services/annie-voice/robot_tools.py` | MODIFY | Map-aware navigation |
| `services/annie-voice/tests/test_robot_tools.py` | MODIFY | Navigation tests |

## Prompt

```
Implement HectorSLAM. Read docs/NEXT-SESSION-HECTORSLAM-IMU.md.
Phase 0 (IMU hardware) is DONE — Pico streams heading at /dev/ttyACM0.
Start at Phase 1: udev rule, imu.py serial reader, slam.py HectorSLAM core.
Then Phase 2 (endpoints), Phase 3 (Annie nav upgrade), Phase 4 (calibrate).
```