Enhance lap timer precision via EKF fusion at 200Hz

## Summary

Upgrade lap timer from **~33ms granularity** (30Hz GPS-only) to **~1-2ms precision** by fusing EKF position at 200Hz with timestamp interpolation.

---

## Problem Statement

### Current Architecture

The lap timer currently runs at 30Hz (telemetry rate) using raw GPS lat/lon:

```
GPS Fix (25Hz, 40ms)
    │
    └──► Lap Timer Update (30Hz, 33ms) ◄── Telemetry rate
              │
              ├── Input: Raw GPS lat/lon (same value repeated for ~40ms)
              ├── Input: EKF velocity (for direction validation only)
              ├── Detection: Line segment intersection
              └── Output: timestamp = now_ms (no interpolation)
```

**The Problem:** We already have EKF position available at 200Hz (IMU rate) with smooth interpolation between GPS fixes, but the lap timer ignores it completely.

```rust
// main.rs:922-923 - INSIDE telemetry block (30Hz), using RAW GPS
let gps_fix = sensors.gps_parser.last_fix();
let lap_flags = lap_timer.update(
    gps_fix.lat, gps_fix.lon,      // ❌ Raw GPS (same value for ~40ms)
    (ekf_vx, ekf_vy),               // ✓ EKF velocity (but only for direction)
    now_ms, speed
);
```

### Current Precision Limitations

| Factor | Value | Impact |
|--------|-------|--------|
| Lap timer update rate | 33ms (30Hz) | Position sampled every 33ms |
| GPS position update rate | 40ms (25Hz) | Same lat/lon repeated between fixes |
| No timestamp interpolation | — | Crossing recorded at detection frame, not actual crossing |
| **Worst case timing error** | **~50-70ms** | At 100 km/h = 1.4-1.9 meters of uncertainty |

---

## Proposed Solution

### New Architecture

```
IMU (200Hz, 5ms)
    │
    └──► EKF Predict ──► EKF Position (local meters, interpolated)
              │
              └──► Lap Timer Update (200Hz, 5ms) ◄── IMU rate
                        │
                        ├── Input: EKF position (x, y meters)
                        ├── Input: EKF velocity (vx, vy m/s)
                        ├── Detection: Line segment intersection with fraction
                        ├── Interpolation: t_cross = t_prev + fraction * dt
                        └── Output: Interpolated timestamp (~1ms precision)
```

### Hybrid Coordinate Strategy

**Storage:** GPS lat/lon (for track persistence across sessions)
**Runtime:** Convert to local meters using session's GPS origin

```
┌─────────────────────────────────────────────────────────────────┐
│  Track Activation Flow                                          │
│                                                                 │
│  1. Load track from IndexedDB (GPS lat/lon coordinates)         │
│  2. Wait for GPS origin to be established                       │
│  3. Convert timing line endpoints to local meters:              │
│     local_x = (lon - origin_lon) * cos(lat) * 111320            │
│     local_y = (lat - origin_lat) * 111320                       │
│  4. Cache local-meter timing line for 200Hz crossing detection  │
└─────────────────────────────────────────────────────────────────┘
```

### Timestamp Interpolation

When line segment intersection is detected, interpolate the exact crossing time:

```
prev_pos ────────────●────────────── curr_pos
                     │
              intersection at
              fraction t ∈ [0,1]

crossing_time = prev_timestamp + t * dt

Example:
  prev_timestamp = 65432 ms
  curr_timestamp = 65437 ms (dt = 5ms at 200Hz)
  intersection fraction t = 0.3

  crossing_time = 65432 + 0.3 * 5 = 65433.5 ms → rounded to 65434 ms
```

---

## Design Decisions

| Decision | Answer | Rationale |
|----------|--------|-----------|
| GPS-only fallback before EKF converges | **Yes** | Ensures lap timing works during cold start |
| Debounce timing | **Keep 2000ms** | Conservative but safe; prevents double-counting same crossing |
| Dashboard precision indicator | **No** | Keep invisible to user; simpler UX |

---

## Technical Implementation

### Phase 1: Add Interpolation Math

**File:** `framework/src/lap_timer.rs`

Add line segment intersection function that returns the fraction:

```rust
/// Line segment intersection returning the fraction along the first segment
/// Returns Some(t) where t ∈ [0,1] indicates where on segment (p1→p2) the crossing occurs
fn line_segment_intersection_with_fraction(
    p1: (f32, f32),
    p2: (f32, f32),
    p3: (f32, f32),
    p4: (f32, f32),
) -> Option<f32> {
    let d1 = (p2.0 - p1.0, p2.1 - p1.1);
    let d2 = (p4.0 - p3.0, p4.1 - p3.1);

    let cross = d1.0 * d2.1 - d1.1 * d2.0;
    if cross.abs() < 1e-10 {
        return None; // Parallel lines
    }

    let d3 = (p1.0 - p3.0, p1.1 - p3.1);
    let t = (d2.0 * d3.1 - d2.1 * d3.0) / cross;
    let u = (d1.0 * d3.1 - d1.1 * d3.0) / cross;

    if t >= 0.0 && t <= 1.0 && u >= 0.0 && u <= 1.0 {
        Some(t)
    } else {
        None
    }
}
```

### Phase 2: Add Local Coordinate Support

**File:** `framework/src/lap_timer.rs`

```rust
/// Timing line in local meters (for high-frequency crossing detection)
struct LocalTimingLine {
    p1: (f32, f32),      // Endpoint 1 in local meters
    p2: (f32, f32),      // Endpoint 2 in local meters
    direction: f32,      // Valid crossing direction (radians)
}

enum LocalTrack {
    Loop { line: LocalTimingLine },
    PointToPoint { start: LocalTimingLine, finish: LocalTimingLine },
}

impl LapTimer {
    /// Convert timing line from GPS to local meters
    /// Call this when activating a track after GPS origin is established
    pub fn set_local_timing_line(&mut self, gps_origin: (f64, f64)) {
        if let Some(ref track) = self.track {
            self.local_track = Some(match track {
                TrackType::Loop { line } => {
                    LocalTrack::Loop {
                        line: line.to_local_line(gps_origin),
                    }
                }
                TrackType::PointToPoint { start, finish } => {
                    LocalTrack::PointToPoint {
                        start: start.to_local_line(gps_origin),
                        finish: finish.to_local_line(gps_origin),
                    }
                }
            });
        }
    }

    pub fn has_local_line(&self) -> bool {
        self.local_track.is_some()
    }
}

impl TimingLine {
    fn to_local_line(&self, origin: (f64, f64)) -> LocalTimingLine {
        let (origin_lat, origin_lon) = origin;
        let cos_lat = (origin_lat * std::f64::consts::PI / 180.0).cos();

        LocalTimingLine {
            p1: (
                ((self.p1_lon - origin_lon) * cos_lat * 111320.0) as f32,
                ((self.p1_lat - origin_lat) * 111320.0) as f32,
            ),
            p2: (
                ((self.p2_lon - origin_lon) * cos_lat * 111320.0) as f32,
                ((self.p2_lat - origin_lat) * 111320.0) as f32,
            ),
            direction: self.direction,
        }
    }
}
```

### Phase 3: Add High-Frequency Update Method

**File:** `framework/src/lap_timer.rs`

```rust
impl LapTimer {
    /// Update lap timer with high-frequency EKF position
    ///
    /// # Arguments
    /// * `ekf_pos` - EKF position in local meters (x, y)
    /// * `ekf_vel` - EKF velocity in m/s (vx, vy) for direction validation
    /// * `timestamp_ms` - Current timestamp in milliseconds
    /// * `dt_ms` - Time delta since last update in milliseconds
    /// * `speed_mps` - Current speed for stationary filtering
    ///
    /// # Returns
    /// Lap flags for this frame
    pub fn update_ekf(
        &mut self,
        ekf_pos: (f32, f32),
        ekf_vel: (f32, f32),
        timestamp_ms: u32,
        dt_ms: f32,
        speed_mps: f32,
    ) -> u8 {
        // Clear per-frame flags
        self.timing.frame_flags = FLAG_NONE;

        // Update current lap time if timing
        if self.timing.state == LapTimerState::Timing {
            self.timing.current_lap_ms = timestamp_ms.saturating_sub(self.timing.lap_start_ms);
        }

        // Need previous position for crossing detection
        let Some(prev_pos) = self.prev_ekf_pos else {
            self.prev_ekf_pos = Some(ekf_pos);
            self.prev_timestamp_ms = timestamp_ms;
            return self.timing.frame_flags;
        };

        // Update previous position for next frame
        self.prev_ekf_pos = Some(ekf_pos);
        let prev_ts = self.prev_timestamp_ms;
        self.prev_timestamp_ms = timestamp_ms;

        // Skip if no local track configured
        let Some(ref local_track) = self.local_track else {
            return self.timing.frame_flags;
        };

        // Skip if nearly stationary (avoid false crossings while parked on line)
        if speed_mps < 0.5 {
            return self.timing.frame_flags;
        }

        // Check for line crossings with interpolated timing
        match local_track {
            LocalTrack::Loop { ref line } => {
                self.check_crossing_interpolated(
                    prev_pos, ekf_pos, ekf_vel, prev_ts, dt_ms, line, true
                );
            }
            LocalTrack::PointToPoint { ref start, ref finish } => {
                // Check start line
                if self.timing.state == LapTimerState::Armed {
                    self.check_crossing_interpolated(
                        prev_pos, ekf_pos, ekf_vel, prev_ts, dt_ms, start, true
                    );
                }
                // Check finish line
                if self.timing.state == LapTimerState::Timing {
                    self.check_crossing_interpolated(
                        prev_pos, ekf_pos, ekf_vel, prev_ts, dt_ms, finish, false
                    );
                }
            }
        }

        self.timing.frame_flags
    }

    fn check_crossing_interpolated(
        &mut self,
        prev_pos: (f32, f32),
        curr_pos: (f32, f32),
        velocity: (f32, f32),
        prev_timestamp_ms: u32,
        dt_ms: f32,
        line: &LocalTimingLine,
        is_start_line: bool,
    ) {
        // Get intersection with fraction
        let Some(fraction) = line_segment_intersection_with_fraction(
            prev_pos, curr_pos,
            line.p1, line.p2
        ) else {
            return;
        };

        // Validate crossing direction
        if !direction_valid(velocity, line.direction, self.direction_tolerance) {
            return;
        }

        // Interpolate timestamp
        let crossing_time = prev_timestamp_ms + (fraction * dt_ms) as u32;

        // Debounce check
        let since_last = crossing_time.saturating_sub(self.timing.last_crossing_ms);
        if since_last < self.crossing_debounce_ms {
            return;
        }
        self.timing.last_crossing_ms = crossing_time;

        // Handle crossing based on state and line type
        // ... (similar logic to existing check_loop_crossing / check_point_to_point_crossing
        //      but using interpolated crossing_time for lap_start_ms)
    }
}
```

### Phase 4: Main Loop Integration

**File:** `sensors/blackbox/src/main.rs`

```rust
// Add state for flag accumulation
let mut accumulated_lap_flags: u8 = 0;
let mut pending_local_conversion = false;

// In the lap timer config handling section:
if let Some(config) = state.take_lap_timer_config() {
    match config {
        LapTimerConfig::Loop(line) => {
            lap_timer.configure_loop(line);
            // Try to convert to local coords if GPS origin available
            if let Some(origin) = sensors.gps_parser.get_origin() {
                lap_timer.set_local_timing_line(origin);
            } else {
                pending_local_conversion = true;
            }
        }
        // ... similar for PointToPoint and Clear ...
    }
}

// Handle deferred local conversion when GPS origin becomes available
if pending_local_conversion && sensors.gps_parser.is_warmed_up() {
    if let Some(origin) = sensors.gps_parser.get_origin() {
        lap_timer.set_local_timing_line(origin);
        pending_local_conversion = false;
        log::info!("Lap timer: converted timing line to local coordinates");
    }
}

// In the IMU polling block (200Hz):
if let Some((dt, _, accel_count)) = sensors.poll_imu() {
    // ... existing EKF predict code ...

    // HIGH-FREQUENCY LAP TIMER UPDATE
    if lap_timer.has_local_line() {
        let (ekf_x, ekf_y) = estimator.ekf.position();
        let (ekf_vx, ekf_vy) = estimator.ekf.velocity();
        let speed = (ekf_vx * ekf_vx + ekf_vy * ekf_vy).sqrt();

        let flags = lap_timer.update_ekf(
            (ekf_x, ekf_y),
            (ekf_vx, ekf_vy),
            now_ms,
            dt * 1000.0,  // Convert seconds to ms
            speed,
        );
        accumulated_lap_flags |= flags;
    }
}

// GPS-ONLY FALLBACK: If no local line yet, use existing GPS-based detection
// This runs at GPS rate when new fixes arrive
if sensors.poll_gps() && !lap_timer.has_local_line() {
    // ... existing GPS-based lap timer update as fallback ...
}

// In the telemetry block (30Hz):
if now_ms - last_telemetry_ms >= config.telemetry.interval_ms {
    // Use accumulated flags from 200Hz updates
    let lap_timer_data = Some((
        lap_timer.current_lap_ms(),
        lap_timer.lap_count(),
        accumulated_lap_flags,
    ));
    accumulated_lap_flags = 0;  // Reset for next telemetry cycle

    // ... rest of telemetry publishing ...
}
```

### Phase 5: New Struct Fields

**File:** `framework/src/lap_timer.rs`

Add to `LapTimer` struct:

```rust
pub struct LapTimer {
    // ... existing fields ...

    // High-frequency EKF tracking
    local_track: Option<LocalTrack>,
    prev_ekf_pos: Option<(f32, f32)>,
    prev_timestamp_ms: u32,
}

impl LapTimer {
    pub fn new() -> Self {
        Self {
            // ... existing initialization ...
            local_track: None,
            prev_ekf_pos: None,
            prev_timestamp_ms: 0,
        }
    }

    pub fn clear(&mut self) {
        // ... existing clear logic ...
        self.local_track = None;
        self.prev_ekf_pos = None;
    }
}
```

---

## Edge Cases

### EKF Not Converged

**Problem:** EKF position unreliable before convergence.

**Solution:** Check `EKF_RESET_DONE` flag and optionally check position covariance:

```rust
if lap_timer.has_local_line() && unsafe { EKF_RESET_DONE } {
    // EKF converged, use high-frequency detection
    lap_timer.update_ekf(...);
} else if !lap_timer.has_local_line() {
    // Fall back to GPS-only detection
    lap_timer.update(gps_lat, gps_lon, ...);
}
```

### GPS Origin Changes Mid-Session

**Problem:** Rare, but GPS origin could be re-established.

**Solution:** Re-convert timing line and clear previous position:

```rust
impl LapTimer {
    pub fn on_gps_origin_changed(&mut self, new_origin: (f64, f64)) {
        self.prev_ekf_pos = None;  // Avoid false crossing from position jump
        self.set_local_timing_line(new_origin);
    }
}
```

### IMU Drift Between GPS Fixes

**Analysis:** At 25Hz GPS, maximum drift window is 40ms. Typical IMU drift ~0.01 m/s² bias → 0.008m in 40ms. Negligible compared to GPS accuracy.

**Conclusion:** Not a concern for crossing detection.

---

## Testing Strategy

### Unit Tests

```rust
#[test]
fn test_intersection_fraction_midpoint() {
    // Line from (0,-5) to (0,5) crossing horizontal line at y=0
    // Crossing at midpoint should return fraction = 0.5
    let frac = line_segment_intersection_with_fraction(
        (0.0, -5.0), (0.0, 5.0),  // Vehicle path
        (-10.0, 0.0), (10.0, 0.0), // Timing line
    );
    assert!((frac.unwrap() - 0.5).abs() < 0.001);
}

#[test]
fn test_interpolated_timestamp() {
    let mut timer = LapTimer::new();
    // Setup timing line at y=0
    timer.local_track = Some(LocalTrack::Loop {
        line: LocalTimingLine {
            p1: (-10.0, 0.0),
            p2: (10.0, 0.0),
            direction: FRAC_PI_2,  // Northward
        }
    });

    // First update at y=-5
    timer.update_ekf((0.0, -5.0), (0.0, 10.0), 1000, 5.0, 10.0);

    // Second update at y=5, crossing at y=0 (midpoint, t=0.5)
    let flags = timer.update_ekf((0.0, 5.0), (0.0, 10.0), 1005, 5.0, 10.0);

    // Crossing should be at 1000 + 0.5*5 = 1002.5 → 1002 or 1003
    assert!(flags & CROSSED_START != 0);
    // Verify interpolated timestamp is ~1002-1003, not 1005
}

#[test]
fn test_direction_rejection() {
    // Crossing in wrong direction should be rejected
}

#[test]
fn test_debounce() {
    // Rapid crossings within debounce window should be ignored
}

#[test]
fn test_gps_fallback() {
    // Without local_track, should use GPS-based detection
}
```

### Simulation Test

Create test that:
1. Generates known trajectory crossing timing line at known time
2. Simulates IMU at 200Hz with noise
3. Simulates GPS at 25Hz
4. Runs EKF
5. Verifies detected crossing time within ±2ms of ground truth

### Real-World Validation

1. Cross timing line at consistent speed multiple times
2. Compare lap times for consistency (should be ±5ms, not ±50ms)
3. Optionally compare against video analysis for absolute accuracy

---

## Success Metrics

| Metric | Current | Target |
|--------|---------|--------|
| Timing granularity | 33ms | 5ms |
| Effective precision | ~50ms | ~2ms |
| Lap time consistency (same line, same speed) | ±50ms | ±5ms |
| Backward compatibility | — | 100% |

---

## Rollback Plan

Changes are additive. To rollback:
1. Move `lap_timer.update()` back to telemetry block
2. Use GPS lat/lon instead of EKF position
3. The existing `update()` method remains functional

---

## Key Insight

The primary benefit is **consistency**, not absolute accuracy.

EKF between GPS fixes provides a **smooth interpolated trajectory**. While absolute position may be off by ~0.5m from GPS error, the trajectory is consistent. For lap timing, what matters is:

> The same crossing point at the same speed should give the same time ±2ms, lap after lap.

This is what enables meaningful comparison between laps.


Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Enhance lap timer precision via EKF fusion at 200Hz #36

Summary

Problem Statement

Current Architecture

Current Precision Limitations

Proposed Solution

New Architecture

Hybrid Coordinate Strategy

Timestamp Interpolation

Design Decisions

Technical Implementation

Phase 1: Add Interpolation Math

Phase 2: Add Local Coordinate Support

Phase 3: Add High-Frequency Update Method

Phase 4: Main Loop Integration

Phase 5: New Struct Fields

Edge Cases

EKF Not Converged

GPS Origin Changes Mid-Session

IMU Drift Between GPS Fixes

Testing Strategy

Unit Tests

Simulation Test

Real-World Validation

Success Metrics

Rollback Plan

Key Insight

Metadata

Assignees

Labels

Projects

Milestone

Relationships

Development

Factor	Value	Impact
Lap timer update rate	33ms (30Hz)	Position sampled every 33ms
GPS position update rate	40ms (25Hz)	Same lat/lon repeated between fixes
No timestamp interpolation	—	Crossing recorded at detection frame, not actual crossing
Worst case timing error	~50-70ms	At 100 km/h = 1.4-1.9 meters of uncertainty

Decision	Answer	Rationale
GPS-only fallback before EKF converges	Yes	Ensures lap timing works during cold start
Debounce timing	Keep 2000ms	Conservative but safe; prevents double-counting same crossing
Dashboard precision indicator	No	Keep invisible to user; simpler UX

Metric	Current	Target
Timing granularity	33ms	5ms
Effective precision	~50ms	~2ms
Lap time consistency (same line, same speed)	±50ms	±5ms
Backward compatibility	—	100%

Uh oh!

Enhance lap timer precision via EKF fusion at 200Hz #36

Description

Summary

Problem Statement

Current Architecture

Current Precision Limitations

Proposed Solution

New Architecture

Hybrid Coordinate Strategy

Timestamp Interpolation

Design Decisions

Technical Implementation

Phase 1: Add Interpolation Math

Phase 2: Add Local Coordinate Support

Phase 3: Add High-Frequency Update Method

Phase 4: Main Loop Integration

Phase 5: New Struct Fields

Edge Cases

EKF Not Converged

GPS Origin Changes Mid-Session

IMU Drift Between GPS Fixes

Testing Strategy

Unit Tests

Simulation Test

Real-World Validation

Success Metrics

Rollback Plan

Key Insight

Metadata

Metadata

Assignees

Labels

Projects

Milestone

Relationships

Development

Issue actions