# Next Session: Fix SLAM Drift with Odometry

## Problem
SLAM reported 6.9m displacement when car moved <1m. 211° heading drift.
Lidar is working perfectly (477 pts/rotation, rplidarc1). The issue is
SLAM has NO odometry — no XY prediction, scan matching freely drifts.

## Root Cause (from code analysis)
1. **No XY prediction in SLAM** — `_slam_step()` only adds IMU heading delta
   to pose. X/Y prediction is unchanged (always last matched pose).
2. **Scan matching fails on sparse map** — `WARMUP_SCANS=3` means matching
   starts after 300ms. Empty map → near-zero gradients → optimizer returns
   the IMU-drifted prediction unchanged → drift baked into map.
3. **No MIN_TRAVEL guard** — every scan updates the map even when stationary.
   Intermediate rotation scans degrade map quality.
4. **IMU gyro drifts ~4°/min** — compounds without scan matching correction.

## Discovery: HiWonder Board Has Encoders + IMU

The STM32 board (`ros_robot_controller_sdk.py`) has:

```python
# Closed-loop speed control (implies encoders exist internally):
board.set_motor_speed([[1, 0.3], [2, 0.3], [3, 0.3], [4, 0.3]])  # m/s

# On-board IMU (ax, ay, az, gx, gy, gz):
board.enable_reception()
imu_data = board.get_imu()  # Returns 6 floats

# We currently use (open-loop, no feedback):
board.set_motor_duty([[1, 50], [2, 50], [3, 50], [4, 50]])  # raw PWM %
```

**Key:** `set_motor_speed` does closed-loop PID on the STM32 with encoder
feedback. We can't READ encoder ticks directly, but if we command a known
velocity for a known duration, we can compute dead-reckoning odometry:
`distance = speed_m_s × duration_s`.

## Implementation Plan

### Phase 1: Commanded-Velocity Odometry (biggest win)

**1a. Switch /drive from `set_motor_duty` to `set_motor_speed`**
- File: `services/turbopi-server/main.py` — `_drive_sync()` function
- Current: `car.set_motor_duty(...)` with raw PWM values
- New: `car.set_motor_speed(...)` with calibrated m/s values
- Need calibration: drive forward 1m, measure actual distance, compute scale
- Mecanum kinematics: forward = all same speed, strafe = alternating signs

**1b. Return odometry from /drive endpoint**
- After drive completes, return `{"dx_m": ..., "dy_m": ..., "dtheta_rad": ...}`
- Computed from: action type × speed × duration × mecanum kinematics
- Forward: dx = speed × duration, dy = 0
- Left/right strafe: dx = 0, dy = ±speed × duration
- Turn: dtheta = angular_speed × duration (need angular calibration)

**1c. Feed odometry into SLAM prediction**
- File: `services/turbopi-server/slam.py` — `_slam_step()`
- Add method: `feed_odometry(dx, dy, dtheta)` — called by /drive endpoint
- In `_slam_step()`: use accumulated odometry for XY prediction (not just IMU heading)
- This gives scan matching a much better initial guess → converges correctly

### Phase 2: SLAM Tuning (complement to odometry)

**2a. Raise WARMUP_SCANS from 3 to 15** (~1.5s of map building before matching)
- File: `slam.py` line 55

**2b. Add match confidence gate**
- After `scan_match_multi_res`, compute match score (sum of map values at scan points)
- If score < threshold: skip map update, don't advance pose
- Prevents baking drift into the grid

**2c. Add stationary guard**
- If no odometry received AND no significant IMU rotation: skip SLAM step
- Prevents accumulating noise from identical scans while parked

### Phase 3: Explore Board IMU (optional)

The board has `get_imu()` returning `(ax, ay, az, gx, gy, gz)`.
- May be better/worse than external MPU-6050
- Test: `board.enable_reception(); board.get_imu()`
- Could replace Pico IMU bridge if quality is good
- Or fuse both for redundancy

### Phase 4: Navigation Cycle Tuning

**4a. Reduce drive duration from 2.0s to 1.0s** (more frequent SLAM corrections)
- File: `services/annie-voice/robot_tools.py` line 598

**4b. Increase max_cycles from 8 to 15-20** (compensate for shorter drives)
- File: `services/annie-voice/robot_tools.py` line 793 (room_explorer default)
- File: `services/annie-voice/tool_schemas.py` line 408 (schema clamp)

**4c. Speed floor already 40** — line 481 clamps speed ≥ 40

## Calibration Needed

Before switching to `set_motor_speed`:
1. Command `set_motor_speed([[1, 0.3], [2, 0.3], [3, 0.3], [4, 0.3]])` for 2s
2. Measure actual distance traveled (tape measure)
3. Compute scale: `actual_m / (0.3 * 2.0)` = correction factor
4. Apply factor to odometry calculation

For turns:
1. Command a spin for 2s at known angular speed
2. Measure actual rotation (IMU or visual)
3. Compute angular scale factor

## Key Files
- `services/turbopi-server/main.py` — /drive endpoint, `_drive_sync()` (~line 728)
- `services/turbopi-server/slam.py` — SLAM: `_slam_step()` (line 541), constants (line 35)
- `services/turbopi-server/imu.py` — IMU reader (consume_heading_delta_deg)
- `services/annie-voice/robot_tools.py` — navigation loop, duration=2.0 (line 598)
- `services/annie-voice/tool_schemas.py` — NavigateRobotInput schema (line 406)
- `/home/rajesh/TurboPi/HiwonderSDK/ros_robot_controller_sdk.py` — Board SDK
- `/home/rajesh/TurboPi/HiwonderSDK/mecanum.py` — mecanum kinematics reference

## Quick Test Commands
```bash
# Test board IMU
ssh pi "cd /home/rajesh/TurboPi/HiwonderSDK && python3 -c \"
from ros_robot_controller_sdk import Board
import time
b = Board()
b.enable_reception()
time.sleep(0.5)
for _ in range(10):
    imu = b.get_imu()
    if imu: print(f'ax={imu[0]:.2f} ay={imu[1]:.2f} az={imu[2]:.2f} gx={imu[3]:.2f} gy={imu[4]:.2f} gz={imu[5]:.2f}')
    time.sleep(0.1)
\""

# Test set_motor_speed (forward 0.2 m/s for 1s)
ssh pi "cd /home/rajesh/TurboPi/HiwonderSDK && python3 -c \"
from ros_robot_controller_sdk import Board
import time
b = Board()
b.set_motor_speed([[1, 0.2], [2, 0.2], [3, 0.2], [4, 0.2]])
time.sleep(1)
b.set_motor_speed([[1, 0], [2, 0], [3, 0], [4, 0]])
print('Done - measure distance traveled')
\""
```

## Git State
Main at `f2a0d63`. Clean working tree. Pi running rplidarc1 lidar (session 63).