From d6812f32d8a45063dd5f666fd6b7d535439978bb Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 15:56:22 -0800 Subject: [PATCH 01/30] Add sensor fusion for noise cleanup and GPS/IMU blending - Add Biquad IIR low-pass filter (2Hz cutoff) for vibration removal - Add GPS-derived longitudinal acceleration from velocity changes - Add configurable GPS/IMU blending (70/50/30% based on GPS rate) - Add tilt estimation for mounting angle correction when stationary - Add gravity estimation for continuous correction during steady driving - Add hybrid mode detection using blended lon accel and filtered lat accel - Update diagnostics with gps_fix_count() for fusion rate tracking --- CLAUDE.md | 95 +++- sensors/blackbox/src/diagnostics.rs | 6 + sensors/blackbox/src/filter.rs | 362 +++++++++++++ sensors/blackbox/src/fusion.rs | 759 ++++++++++++++++++++++++++++ sensors/blackbox/src/main.rs | 65 ++- sensors/blackbox/src/mode.rs | 86 ++++ 6 files changed, 1358 insertions(+), 15 deletions(-) create mode 100644 sensors/blackbox/src/filter.rs create mode 100644 sensors/blackbox/src/fusion.rs diff --git a/CLAUDE.md b/CLAUDE.md index dd13468..7a689a5 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -531,29 +531,98 @@ Result: acceleration due to motion only (no gravity component). ### mode.rs - Driving Mode Classifier -**NEW: Independent Component Detection** (replaces exclusive state machine) +**Hybrid Mode Detection** with GPS/IMU blending: +- Longitudinal acceleration (ACCEL/BRAKE) uses **blended GPS + IMU** +- Lateral acceleration (CORNER) uses **filtered IMU** (GPS can't sense lateral) +- GPS-derived acceleration computed from velocity changes at 25Hz +- Blending ratio adapts based on GPS rate and freshness + ``` -Each mode component detected independently with hysteresis: +Independent hysteresis detection: -ACCEL: entry at 0.10g → exit at 0.05g -BRAKE: entry at -0.18g → exit at -0.09g (mutually exclusive with ACCEL) -CORNER: entry at 0.12g lat + 0.05rad/s yaw → exit at 0.06g lat or low yaw +ACCEL: entry at 0.10g → exit at 0.05g (from blended lon accel) +BRAKE: entry at -0.18g → exit at -0.09g (from blended lon accel) +CORNER: entry at 0.12g lat + 0.05rad/s yaw → exit at 0.06g lat -Combined states possible: +Combined states: - ACCEL + CORNER = corner exit (trail throttle) - BRAKE + CORNER = corner entry (trail braking) ``` -**Hysteresis:** Entry threshold > exit threshold prevents oscillation. +**Benefits of Hybrid Detection:** +- Mounting angle independent (GPS measures actual vehicle acceleration) +- No drift (GPS doesn't drift like IMU) +- Graceful fallback when GPS degrades **All modes require:** speed > min_speed (2.0 m/s / 7.2 km/h) -**Speed Display:** -Uses **raw GPS speed** directly (system.rs:295-297) for responsive updates. -- No longer uses EKF-filtered velocity (which was laggy) -- Server sends raw GPS speed (5 Hz updates) -- Dashboard applies light EMA smoothing (alpha=0.4) for car speedometer-like display -- Mode detection still uses EKF velocity for stability +### filter.rs - Biquad Low-Pass Filter + +2nd-order Butterworth IIR filter for vibration removal. + +**Purpose:** +- Remove engine vibration (30-100 Hz) +- Remove road texture noise (5-20 Hz) +- Preserve driving dynamics (0-2 Hz) + +**Configuration:** +- Sample rate: 20 Hz (telemetry rate) +- Cutoff: 2 Hz +- Q = 0.707 (critically damped, no overshoot) + +**Implementation:** +```rust +// Direct Form I biquad +y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] +``` + +### fusion.rs - Sensor Fusion Module + +Handles GPS/IMU blending, tilt correction, and continuous calibration. + +**Components:** + +1. **GpsAcceleration** - Computes longitudinal acceleration from GPS velocity + - `accel = (speed_new - speed_old) / dt` + - At 25Hz GPS: dt = 40ms, clean acceleration signal + - Tracks staleness for fallback to IMU + +2. **TiltEstimator** - Learns mounting offset when stopped + - After 3 seconds stationary, averages earth-frame acceleration + - This average IS the mounting error (gravity leakage from tilt) + - Applies correction to all future readings + - Relearns at every stop (adapts to device repositioning) + +3. **GravityEstimator** - Learns gravity offset while driving + - Detects "steady state": constant speed (±0.5 m/s), low yaw (±5°/s) + - During steady state, expected acceleration ≈ 0 + - Slowly updates gravity estimate (α=0.02, ~50 second convergence) + - Essential for track/canyon driving where stops are rare + +4. **SensorFusion** - Main processor + - Applies tilt correction + - Applies gravity correction + - Low-pass filters to remove vibration + - Blends GPS and IMU for longitudinal acceleration + +**GPS/IMU Blending Ratios:** +``` +GPS rate >= 20Hz, fresh: 90% GPS / 10% IMU (high confidence) +GPS rate 10-20Hz: 70% GPS / 30% IMU (medium confidence) +GPS rate < 10Hz: 30% GPS / 70% IMU (low confidence) +GPS stale (>200ms): 0% GPS / 100% IMU (fallback) +``` + +**Data Flow:** +``` +GPS (25Hz) → GpsAcceleration → lon_accel_gps + ↓ +IMU → remove_gravity → body_to_earth → TiltCorrect → GravityCorrect → Biquad Filter + ↓ + lon_accel_imu + ↓ + Blend(gps, imu) → mode.rs +``` ### websocket_server.rs - HTTP Dashboard Server diff --git a/sensors/blackbox/src/diagnostics.rs b/sensors/blackbox/src/diagnostics.rs index 3fd968a..747193d 100644 --- a/sensors/blackbox/src/diagnostics.rs +++ b/sensors/blackbox/src/diagnostics.rs @@ -199,6 +199,12 @@ impl DiagnosticsState { self.zupt_count.fetch_add(1, Ordering::Relaxed); } + /// Get total GPS fix count (for sensor fusion rate tracking) + #[inline] + pub fn gps_fix_count(&self) -> u32 { + self.gps_fix_count.load(Ordering::Relaxed) + } + /// Update sensor rates (call periodically, e.g., every second) pub fn update_rates(&self, now_ms: u32) { let last_ms = self.last_rate_check_ms.swap(now_ms, Ordering::SeqCst); diff --git a/sensors/blackbox/src/filter.rs b/sensors/blackbox/src/filter.rs new file mode 100644 index 0000000..5882c65 --- /dev/null +++ b/sensors/blackbox/src/filter.rs @@ -0,0 +1,362 @@ +/// Biquad IIR low-pass filter for vibration removal +/// +/// Implements a 2nd-order Butterworth low-pass filter optimized for +/// removing engine/road vibration (10-100Hz) while preserving +/// driving dynamics (0-3Hz). +/// +/// Design: Butterworth (maximally flat passband, Q = 0.707) +/// Typical cutoff: 4Hz at 200Hz sample rate + +#[derive(Clone)] +pub struct BiquadFilter { + // Filter coefficients (normalized) + b0: f32, + b1: f32, + b2: f32, + a1: f32, + a2: f32, + + // Filter state (previous samples) + x1: f32, // x[n-1] + x2: f32, // x[n-2] + y1: f32, // y[n-1] + y2: f32, // y[n-2] +} + +impl BiquadFilter { + /// Create a new Butterworth low-pass filter + /// + /// # Arguments + /// * `cutoff_hz` - Cutoff frequency in Hz (e.g., 4.0) + /// * `sample_rate_hz` - Sample rate in Hz (e.g., 200.0) + /// + /// # Panics + /// Panics if cutoff >= sample_rate/2 (Nyquist limit) + pub fn new_lowpass(cutoff_hz: f32, sample_rate_hz: f32) -> Self { + assert!( + cutoff_hz < sample_rate_hz / 2.0, + "Cutoff must be below Nyquist frequency" + ); + + // Butterworth Q factor for critically damped response + let q = core::f32::consts::FRAC_1_SQRT_2; // 0.7071... + + // Pre-warp the cutoff frequency for bilinear transform + let omega = 2.0 * core::f32::consts::PI * cutoff_hz / sample_rate_hz; + let sin_omega = omega.sin(); + let cos_omega = omega.cos(); + let alpha = sin_omega / (2.0 * q); + + // Compute coefficients (lowpass) + let b0 = (1.0 - cos_omega) / 2.0; + let b1 = 1.0 - cos_omega; + let b2 = (1.0 - cos_omega) / 2.0; + let a0 = 1.0 + alpha; + let a1 = -2.0 * cos_omega; + let a2 = 1.0 - alpha; + + // Normalize by a0 + Self { + b0: b0 / a0, + b1: b1 / a0, + b2: b2 / a0, + a1: a1 / a0, + a2: a2 / a0, + x1: 0.0, + x2: 0.0, + y1: 0.0, + y2: 0.0, + } + } + + /// Process a single sample through the filter + /// + /// # Arguments + /// * `input` - Input sample + /// + /// # Returns + /// Filtered output sample + pub fn process(&mut self, input: f32) -> f32 { + // Direct Form I implementation + // y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] + let output = self.b0 * input + self.b1 * self.x1 + self.b2 * self.x2 + - self.a1 * self.y1 + - self.a2 * self.y2; + + // Update state + self.x2 = self.x1; + self.x1 = input; + self.y2 = self.y1; + self.y1 = output; + + output + } + + /// Initialize filter state to a steady value + /// + /// Use this to avoid startup transients when you know + /// the initial value (e.g., after calibration shows ~0) + pub fn reset_to(&mut self, value: f32) { + self.x1 = value; + self.x2 = value; + self.y1 = value; + self.y2 = value; + } +} + +#[cfg(test)] +mod tests { + use super::*; + + // Test with IMU-rate parameters (200Hz sampling, 4Hz cutoff) + const SAMPLE_RATE: f32 = 200.0; + const CUTOFF: f32 = 4.0; + + // Also test with telemetry-rate parameters (20Hz sampling, 2Hz cutoff) + const TELEM_RATE: f32 = 20.0; + const TELEM_CUTOFF: f32 = 2.0; + + #[test] + fn test_filter_creation() { + let filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + // Verify coefficients are reasonable + assert!(filter.b0 > 0.0 && filter.b0 < 1.0); + assert!(filter.b1 > 0.0); + assert!(filter.b2 > 0.0 && filter.b2 < 1.0); + } + + #[test] + #[should_panic(expected = "Cutoff must be below Nyquist")] + fn test_filter_nyquist_limit() { + // Should panic: cutoff >= sample_rate/2 + let _ = BiquadFilter::new_lowpass(100.0, SAMPLE_RATE); + } + + #[test] + fn test_dc_passthrough() { + // DC signal (0 Hz) should pass through unchanged + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + // Feed constant value, let filter settle + for _ in 0..100 { + filter.process(1.0); + } + + // After settling, output should be ~1.0 + let output = filter.process(1.0); + assert!( + (output - 1.0).abs() < 0.01, + "DC should pass through: got {}", + output + ); + } + + #[test] + fn test_high_frequency_attenuation() { + // 50Hz signal should be heavily attenuated (4Hz cutoff) + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + // Generate 50Hz sine wave + let freq = 50.0; + let mut max_output: f32 = 0.0; + + // Let filter settle, then measure + for i in 0..1000 { + let t = i as f32 / SAMPLE_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 500 { + // After settling + max_output = max_output.max(output.abs()); + } + } + + // At 50Hz with 4Hz cutoff, attenuation should be significant + // 50Hz is ~3.6 octaves above 4Hz, expect -40dB or more + // That's amplitude ratio of 0.01 or less + assert!( + max_output < 0.05, + "50Hz should be attenuated: got peak {}", + max_output + ); + } + + #[test] + fn test_low_frequency_passthrough() { + // 1Hz signal should pass through mostly unchanged + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let freq = 1.0; + let mut max_output: f32 = 0.0; + + for i in 0..2000 { + // Need longer for 1Hz signal + let t = i as f32 / SAMPLE_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 1000 { + max_output = max_output.max(output.abs()); + } + } + + // 1Hz is well below 4Hz cutoff, should pass with minimal attenuation + assert!( + max_output > 0.9, + "1Hz should pass through: got peak {}", + max_output + ); + } + + #[test] + fn test_step_response_no_overshoot() { + // Butterworth should have no overshoot (critically damped) + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let mut max_output: f32 = 0.0; + + // Step from 0 to 1 + for _ in 0..500 { + let output = filter.process(1.0); + max_output = max_output.max(output); + } + + // Should never exceed 1.0 (no overshoot) + assert!( + max_output <= 1.01, + "Should not overshoot: got max {}", + max_output + ); + } + + #[test] + fn test_reset_to_value() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + // Reset to steady state of 0.5 + filter.reset_to(0.5); + + // Immediately processing 0.5 should output ~0.5 + let output = filter.process(0.5); + assert!( + (output - 0.5).abs() < 0.01, + "After reset_to, should output steady value: got {}", + output + ); + } + + #[test] + fn test_vibration_simulation() { + // Simulate realistic scenario: driving signal + engine vibration + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let driving_freq = 0.5; // 0.5 Hz - slow braking + let vibration_freq = 60.0; // 60 Hz - engine vibration + let driving_amplitude = 0.3; // 0.3g braking + let vibration_amplitude = 0.05; // 0.05g vibration + + let mut driving_energy = 0.0; + let mut vibration_energy = 0.0; + let mut output_energy = 0.0; + + for i in 0..4000 { + // 20 seconds + let t = i as f32 / SAMPLE_RATE; + + // Input: driving signal + vibration noise + let driving = driving_amplitude * (2.0 * core::f32::consts::PI * driving_freq * t).sin(); + let vibration = + vibration_amplitude * (2.0 * core::f32::consts::PI * vibration_freq * t).sin(); + let input = driving + vibration; + + let output = filter.process(input); + + if i > 1000 { + // After settling + driving_energy += driving * driving; + vibration_energy += vibration * vibration; + output_energy += output * output; + } + } + + // Output energy should be close to driving energy (vibration removed) + let ratio = output_energy / driving_energy; + assert!( + ratio > 0.8 && ratio < 1.2, + "Filter should preserve driving signal: energy ratio = {}", + ratio + ); + + // Output should have much less energy than input (vibration removed) + let input_energy = driving_energy + vibration_energy; + assert!( + output_energy < input_energy * 0.95, + "Filter should remove vibration energy" + ); + } + + #[test] + fn test_telemetry_rate_filter() { + // Test at telemetry rate (20Hz sampling, 2Hz cutoff) + // This is the actual configuration used in main.rs + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF, TELEM_RATE); + + // DC should pass through + for _ in 0..50 { + filter.process(1.0); + } + let dc_output = filter.process(1.0); + assert!( + (dc_output - 1.0).abs() < 0.02, + "DC should pass at telemetry rate: got {}", + dc_output + ); + + // Reset and test low frequency (0.5 Hz driving input) + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF, TELEM_RATE); + let freq = 0.5; // 0.5 Hz - typical braking/accel + let mut max_output: f32 = 0.0; + + for i in 0..200 { + // 10 seconds at 20Hz + let t = i as f32 / TELEM_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 100 { + max_output = max_output.max(output.abs()); + } + } + + // 0.5 Hz is well below 2Hz cutoff, should pass through + assert!( + max_output > 0.85, + "0.5Hz should pass at 20Hz/2Hz filter: got {}", + max_output + ); + + // Test high frequency rejection (8Hz - at Nyquist, should be attenuated) + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF, TELEM_RATE); + let freq = 8.0; + let mut max_output: f32 = 0.0; + + for i in 0..200 { + let t = i as f32 / TELEM_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 100 { + max_output = max_output.max(output.abs()); + } + } + + // 8Hz is well above 2Hz cutoff at 20Hz sample rate + assert!( + max_output < 0.2, + "8Hz should be attenuated at 20Hz/2Hz filter: got {}", + max_output + ); + } +} + diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs new file mode 100644 index 0000000..76b0bb5 --- /dev/null +++ b/sensors/blackbox/src/fusion.rs @@ -0,0 +1,759 @@ +/// Sensor Fusion Module +/// +/// This module handles: +/// 1. GPS-derived longitudinal acceleration (from velocity changes) +/// 2. GPS/IMU acceleration blending with adaptive weights +/// 3. Dynamic tilt offset learning (ZUPT enhancement) +/// 4. Moving gravity estimation (continuous calibration while driving) +/// +/// The goal is to provide clean, drift-free acceleration for mode detection +/// that works regardless of device mounting angle. + +use crate::filter::BiquadFilter; + +/// Configuration for sensor fusion +#[derive(Clone, Copy)] +pub struct FusionConfig { + /// GPS rate threshold for high-confidence GPS (Hz) + pub gps_high_rate: f32, + /// GPS rate threshold for medium-confidence GPS (Hz) + pub gps_medium_rate: f32, + /// Maximum age of GPS data before considered stale (seconds) + pub gps_max_age: f32, + /// GPS weight when rate >= high_rate (0.0-1.0) + /// Higher = more GPS, smoother but slower response + /// Lower = more IMU, faster response but potential drift + pub gps_weight_high: f32, + /// GPS weight when rate >= medium_rate (0.0-1.0) + pub gps_weight_medium: f32, + /// GPS weight when rate < medium_rate (0.0-1.0) + pub gps_weight_low: f32, + /// Time required at stop to learn tilt offset (seconds) + pub tilt_learn_time: f32, + /// Speed threshold for steady-state detection (m/s) + pub steady_state_speed_tolerance: f32, + /// Yaw rate threshold for steady-state (rad/s) + pub steady_state_yaw_tolerance: f32, + /// Time required in steady state to update gravity estimate (seconds) + pub gravity_learn_time: f32, + /// Alpha for gravity estimate update (smaller = slower, more stable) + pub gravity_alpha: f32, + /// Low-pass filter cutoff for acceleration (Hz) + pub accel_filter_cutoff: f32, + /// IMU sample rate (Hz) + pub imu_sample_rate: f32, +} + +impl Default for FusionConfig { + /// Default configuration balanced for city/highway/canyon driving. + /// + /// GPS weights are moderate (70/50/30) for balanced responsiveness: + /// - Fast enough for canyon driving (~80-100ms latency) + /// - Smooth enough for city/highway (no jitter) + /// + /// For track use, consider increasing GPS weights (90/70/50) for + /// maximum accuracy at the cost of some latency. + fn default() -> Self { + Self { + gps_high_rate: 20.0, + gps_medium_rate: 10.0, + gps_max_age: 0.2, // 200ms + // Balanced blend: 70% GPS when fresh and fast + // Gives ~80-100ms latency instead of ~120-150ms + gps_weight_high: 0.70, // 70% GPS / 30% IMU at >= 20Hz + gps_weight_medium: 0.50, // 50% GPS / 50% IMU at >= 10Hz + gps_weight_low: 0.30, // 30% GPS / 70% IMU at < 10Hz + tilt_learn_time: 3.0, + steady_state_speed_tolerance: 0.5, // 0.5 m/s = 1.8 km/h + steady_state_yaw_tolerance: 0.087, // ~5 deg/s + gravity_learn_time: 2.0, + gravity_alpha: 0.02, // Very slow update + accel_filter_cutoff: 4.0, + imu_sample_rate: 200.0, + } + } +} + +/// GPS-derived acceleration tracker +pub struct GpsAcceleration { + /// Previous speed (m/s) + prev_speed: f32, + /// Previous timestamp (seconds, monotonic) + prev_time: f32, + /// Computed longitudinal acceleration (m/s²) + accel_lon: f32, + /// Is the acceleration estimate valid? + valid: bool, + /// Current GPS rate estimate (Hz) + gps_rate: f32, + /// Time since last valid GPS fix (seconds) + time_since_fix: f32, + /// EMA for GPS rate calculation + rate_ema: f32, + /// Last fix count for rate calculation + last_fix_count: u32, + /// Time of last rate calculation + last_rate_time: f32, +} + +impl GpsAcceleration { + pub fn new() -> Self { + Self { + prev_speed: 0.0, + prev_time: 0.0, + accel_lon: 0.0, + valid: false, + gps_rate: 0.0, + time_since_fix: 999.0, + rate_ema: 0.0, + last_fix_count: 0, + last_rate_time: 0.0, + } + } + + /// Update with new GPS speed measurement + /// + /// # Arguments + /// * `speed` - Current speed in m/s + /// * `time` - Current timestamp in seconds (monotonic) + /// + /// # Returns + /// The computed longitudinal acceleration, or None if not enough data + pub fn update(&mut self, speed: f32, time: f32) -> Option { + self.time_since_fix = 0.0; + + if self.prev_time > 0.0 { + let dt = time - self.prev_time; + + // Validate dt: should be 20-200ms for 5-50Hz GPS + if dt > 0.02 && dt < 0.5 { + self.accel_lon = (speed - self.prev_speed) / dt; + self.valid = true; + + self.prev_speed = speed; + self.prev_time = time; + + return Some(self.accel_lon); + } + } + + self.prev_speed = speed; + self.prev_time = time; + None + } + + /// Update GPS rate estimate + /// + /// # Arguments + /// * `fix_count` - Total number of valid fixes received + /// * `time` - Current timestamp in seconds + pub fn update_rate(&mut self, fix_count: u32, time: f32) { + if self.last_rate_time > 0.0 { + let dt = time - self.last_rate_time; + if dt >= 1.0 { + let fixes = fix_count.saturating_sub(self.last_fix_count); + let instant_rate = fixes as f32 / dt; + + // EMA smoothing for rate + self.rate_ema = 0.3 * instant_rate + 0.7 * self.rate_ema; + self.gps_rate = self.rate_ema; + + self.last_fix_count = fix_count; + self.last_rate_time = time; + } + } else { + self.last_fix_count = fix_count; + self.last_rate_time = time; + } + } + + /// Advance time without GPS fix (for staleness tracking) + pub fn advance_time(&mut self, dt: f32) { + self.time_since_fix += dt; + if self.time_since_fix > 0.5 { + self.valid = false; + } + } + + /// Get current GPS-derived acceleration + pub fn get_accel(&self) -> Option { + if self.valid { + Some(self.accel_lon) + } else { + None + } + } + + /// Get current GPS rate estimate (Hz) + pub fn get_rate(&self) -> f32 { + self.gps_rate + } + + /// Check if GPS data is fresh + pub fn is_fresh(&self, max_age: f32) -> bool { + self.time_since_fix < max_age + } +} + +impl Default for GpsAcceleration { + fn default() -> Self { + Self::new() + } +} + +/// Tilt offset estimator - learns mounting offset when stopped +pub struct TiltEstimator { + /// Accumulated X acceleration (earth frame) + accel_x_sum: f32, + /// Accumulated Y acceleration (earth frame) + accel_y_sum: f32, + /// Number of samples accumulated + sample_count: u32, + /// Time stationary (seconds) + stationary_time: f32, + /// Learned tilt offset X (m/s²) + offset_x: f32, + /// Learned tilt offset Y (m/s²) + offset_y: f32, + /// Is the offset valid (learned at least once)? + offset_valid: bool, + /// Required stationary time to learn (seconds) + learn_time: f32, +} + +impl TiltEstimator { + pub fn new(learn_time: f32) -> Self { + Self { + accel_x_sum: 0.0, + accel_y_sum: 0.0, + sample_count: 0, + stationary_time: 0.0, + offset_x: 0.0, + offset_y: 0.0, + offset_valid: false, + learn_time, + } + } + + /// Update with acceleration sample while stationary + /// + /// # Arguments + /// * `ax_earth` - X acceleration in earth frame (m/s²) + /// * `ay_earth` - Y acceleration in earth frame (m/s²) + /// * `dt` - Time step (seconds) + /// + /// # Returns + /// true if offset was just updated (can trigger visual feedback) + pub fn update_stationary(&mut self, ax_earth: f32, ay_earth: f32, dt: f32) -> bool { + self.stationary_time += dt; + self.accel_x_sum += ax_earth; + self.accel_y_sum += ay_earth; + self.sample_count += 1; + + // After sufficient time, compute offset + if self.stationary_time >= self.learn_time && self.sample_count > 50 { + let new_offset_x = self.accel_x_sum / self.sample_count as f32; + let new_offset_y = self.accel_y_sum / self.sample_count as f32; + + // Only update if change is significant (avoid jitter) + let change = ((new_offset_x - self.offset_x).powi(2) + + (new_offset_y - self.offset_y).powi(2)) + .sqrt(); + + if change > 0.05 || !self.offset_valid { + self.offset_x = new_offset_x; + self.offset_y = new_offset_y; + self.offset_valid = true; + + // Reset accumulators for continued learning + self.accel_x_sum = 0.0; + self.accel_y_sum = 0.0; + self.sample_count = 0; + + return true; + } + } + + false + } + + /// Reset stationary tracking (called when vehicle starts moving) + pub fn reset_stationary(&mut self) { + self.accel_x_sum = 0.0; + self.accel_y_sum = 0.0; + self.sample_count = 0; + self.stationary_time = 0.0; + } + + /// Apply tilt correction to acceleration + pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { + if self.offset_valid { + (ax_earth - self.offset_x, ay_earth - self.offset_y) + } else { + (ax_earth, ay_earth) + } + } +} + +/// Moving gravity estimator - learns gravity offset while driving +pub struct GravityEstimator { + /// Estimated gravity offset X (earth frame, m/s²) + offset_x: f32, + /// Estimated gravity offset Y (earth frame, m/s²) + offset_y: f32, + /// Is the estimate valid? + valid: bool, + /// Speed history for steady-state detection + speed_history: [f32; 10], + speed_idx: usize, + /// Time in steady state (seconds) + steady_time: f32, + /// Configuration + speed_tolerance: f32, + yaw_tolerance: f32, + learn_time: f32, + alpha: f32, +} + +impl GravityEstimator { + pub fn new(config: &FusionConfig) -> Self { + Self { + offset_x: 0.0, + offset_y: 0.0, + valid: false, + speed_history: [0.0; 10], + speed_idx: 0, + steady_time: 0.0, + speed_tolerance: config.steady_state_speed_tolerance, + yaw_tolerance: config.steady_state_yaw_tolerance, + learn_time: config.gravity_learn_time, + alpha: config.gravity_alpha, + } + } + + /// Update with current driving state + /// + /// # Arguments + /// * `ax_earth` - X acceleration (earth frame, m/s²) + /// * `ay_earth` - Y acceleration (earth frame, m/s²) + /// * `speed` - Current speed (m/s) + /// * `yaw_rate` - Current yaw rate (rad/s) + /// * `dt` - Time step (seconds) + pub fn update( + &mut self, + ax_earth: f32, + ay_earth: f32, + speed: f32, + yaw_rate: f32, + dt: f32, + ) -> bool { + // Update speed history + self.speed_history[self.speed_idx] = speed; + self.speed_idx = (self.speed_idx + 1) % self.speed_history.len(); + + // Check if in steady state (constant velocity) + let speed_stable = self.is_speed_stable(); + let yaw_low = yaw_rate.abs() < self.yaw_tolerance; + let moving = speed > 2.0; // Must be moving (not stopped) + + if speed_stable && yaw_low && moving { + self.steady_time += dt; + + if self.steady_time >= self.learn_time { + // In steady state, expected acceleration is ~0 + // Any measured acceleration is gravity/mounting error + self.offset_x = (1.0 - self.alpha) * self.offset_x + self.alpha * ax_earth; + self.offset_y = (1.0 - self.alpha) * self.offset_y + self.alpha * ay_earth; + self.valid = true; + return true; + } + } else { + self.steady_time = 0.0; + } + + false + } + + fn is_speed_stable(&self) -> bool { + if self.speed_history[0] < 1.0 { + return false; // Need some speed data + } + + let mean: f32 = self.speed_history.iter().sum::() / self.speed_history.len() as f32; + let max_dev = self + .speed_history + .iter() + .map(|&s| (s - mean).abs()) + .fold(0.0f32, |a, b| a.max(b)); + + max_dev < self.speed_tolerance + } + + /// Apply gravity correction to acceleration + pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { + if self.valid { + (ax_earth - self.offset_x, ay_earth - self.offset_y) + } else { + (ax_earth, ay_earth) + } + } +} + +/// Main sensor fusion processor +pub struct SensorFusion { + pub config: FusionConfig, + /// GPS-derived acceleration + pub gps_accel: GpsAcceleration, + /// Tilt estimator (learns when stopped) + pub tilt_estimator: TiltEstimator, + /// Gravity estimator (learns while driving) + pub gravity_estimator: GravityEstimator, + /// Low-pass filter for IMU X + filter_x: BiquadFilter, + /// Low-pass filter for IMU Y + filter_y: BiquadFilter, + /// Filtered IMU longitudinal acceleration (m/s²) + imu_lon_filtered: f32, + /// Filtered IMU lateral acceleration (m/s²) + imu_lat_filtered: f32, + /// Blended longitudinal acceleration (m/s²) + blended_lon: f32, + /// Last GPS blend weight (for diagnostics) + last_gps_weight: f32, + /// Was stationary last update? + was_stationary: bool, +} + +impl SensorFusion { + pub fn new(config: FusionConfig) -> Self { + Self { + gps_accel: GpsAcceleration::new(), + tilt_estimator: TiltEstimator::new(config.tilt_learn_time), + gravity_estimator: GravityEstimator::new(&config), + filter_x: BiquadFilter::new_lowpass(config.accel_filter_cutoff, config.imu_sample_rate), + filter_y: BiquadFilter::new_lowpass(config.accel_filter_cutoff, config.imu_sample_rate), + imu_lon_filtered: 0.0, + imu_lat_filtered: 0.0, + blended_lon: 0.0, + last_gps_weight: 0.0, + was_stationary: false, + config, + } + } + + /// Process IMU data (call at IMU rate, e.g., 200Hz) + /// + /// # Arguments + /// * `ax_earth` - X acceleration in earth frame (m/s²) + /// * `ay_earth` - Y acceleration in earth frame (m/s²) + /// * `yaw` - Vehicle yaw angle (rad, for earth-to-car transform) + /// * `dt` - Time step (seconds) + /// * `is_stationary` - Whether vehicle is currently stationary + /// + /// # Returns + /// (lon_accel, lat_accel) - Fused accelerations in vehicle frame (m/s²) + pub fn process_imu( + &mut self, + ax_earth: f32, + ay_earth: f32, + yaw: f32, + speed: f32, + yaw_rate: f32, + dt: f32, + is_stationary: bool, + ) -> (f32, f32) { + // Track GPS staleness + self.gps_accel.advance_time(dt); + + // Apply tilt correction (learned when stopped) + let (ax_tilt, ay_tilt) = self.tilt_estimator.correct(ax_earth, ay_earth); + + // Apply gravity correction (learned while driving) + let (ax_grav, ay_grav) = self.gravity_estimator.correct(ax_tilt, ay_tilt); + + // Low-pass filter to remove vibration + let ax_filt = self.filter_x.process(ax_grav); + let ay_filt = self.filter_y.process(ay_grav); + + // Transform to vehicle frame + let (lon_imu, lat_imu) = earth_to_car(ax_filt, ay_filt, yaw); + + self.imu_lon_filtered = lon_imu; + self.imu_lat_filtered = lat_imu; + + // Handle stationary state transitions + if is_stationary { + // Learn tilt offset when stopped + self.tilt_estimator + .update_stationary(ax_earth, ay_earth, dt); + + if !self.was_stationary { + // Just stopped - reset filters to avoid transient + self.filter_x.reset_to(0.0); + self.filter_y.reset_to(0.0); + } + } else { + if self.was_stationary { + // Just started moving + self.tilt_estimator.reset_stationary(); + } + + // Update gravity estimate while driving + self.gravity_estimator + .update(ax_earth, ay_earth, speed, yaw_rate, dt); + } + self.was_stationary = is_stationary; + + // Blend GPS and IMU for longitudinal acceleration + let gps_weight = self.compute_gps_weight(); + self.last_gps_weight = gps_weight; + + let lon_blended = if let Some(gps_lon) = self.gps_accel.get_accel() { + gps_weight * gps_lon + (1.0 - gps_weight) * lon_imu + } else { + lon_imu + }; + + self.blended_lon = lon_blended; + + // Lateral always from IMU (GPS can't sense lateral well) + (lon_blended, lat_imu) + } + + /// Process GPS speed update (call at GPS rate, e.g., 25Hz) + /// + /// # Arguments + /// * `speed` - GPS speed in m/s + /// * `time` - Monotonic timestamp in seconds + pub fn process_gps(&mut self, speed: f32, time: f32) { + self.gps_accel.update(speed, time); + } + + /// Update GPS rate estimate (call periodically, e.g., every second) + pub fn update_gps_rate(&mut self, fix_count: u32, time: f32) { + self.gps_accel.update_rate(fix_count, time); + } + + /// Compute GPS weight based on GPS rate and data freshness + fn compute_gps_weight(&self) -> f32 { + let rate = self.gps_accel.get_rate(); + let fresh = self.gps_accel.is_fresh(self.config.gps_max_age); + + if !fresh { + return 0.0; + } + + // Blending based on GPS rate using configurable weights + if rate >= self.config.gps_high_rate { + self.config.gps_weight_high + } else if rate >= self.config.gps_medium_rate { + self.config.gps_weight_medium + } else if rate > 0.0 { + self.config.gps_weight_low + } else { + 0.0 + } + } +} + +impl Default for SensorFusion { + fn default() -> Self { + Self::new(FusionConfig::default()) + } +} + +/// Earth to car frame transformation +/// +/// Converts earth-frame accelerations to vehicle-frame (longitudinal/lateral) +/// +/// # Arguments +/// * `ax_earth` - X acceleration in earth frame (m/s²) +/// * `ay_earth` - Y acceleration in earth frame (m/s²) +/// * `yaw` - Vehicle heading (rad, 0 = East, π/2 = North) +/// +/// # Returns +/// (lon, lat) - Longitudinal (forward+) and lateral (left+) accelerations +fn earth_to_car(ax_earth: f32, ay_earth: f32, yaw: f32) -> (f32, f32) { + let cos_yaw = yaw.cos(); + let sin_yaw = yaw.sin(); + + // Rotate from earth to car frame + let lon = ax_earth * cos_yaw + ay_earth * sin_yaw; + let lat = -ax_earth * sin_yaw + ay_earth * cos_yaw; + + (lon, lat) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_gps_acceleration_basic() { + let mut gps = GpsAcceleration::new(); + + // First update - no acceleration yet + assert!(gps.update(10.0, 0.0).is_none()); + + // Second update - should compute acceleration + let accel = gps.update(12.0, 0.1); // 2 m/s increase over 0.1s = 20 m/s² + assert!(accel.is_some()); + assert!((accel.unwrap() - 20.0).abs() < 0.1); + } + + #[test] + fn test_gps_acceleration_decel() { + let mut gps = GpsAcceleration::new(); + + gps.update(20.0, 0.0); + let accel = gps.update(15.0, 0.1); // 5 m/s decrease = -50 m/s² + + assert!(accel.is_some()); + assert!((accel.unwrap() + 50.0).abs() < 0.1); + } + + #[test] + fn test_gps_staleness() { + let mut gps = GpsAcceleration::new(); + let config = FusionConfig::default(); + + gps.update(10.0, 0.0); + gps.update(12.0, 0.1); + + assert!(gps.is_fresh(config.gps_max_age)); + + // Advance time without GPS update + gps.advance_time(0.3); // > 200ms + assert!(!gps.is_fresh(config.gps_max_age)); + assert!(gps.get_accel().is_none()); + } + + #[test] + fn test_tilt_estimator_learns() { + let mut tilt = TiltEstimator::new(1.0); // 1 second learn time + + // Simulate stationary with 0.5 m/s² offset + let mut learned = false; + for _ in 0..300 { + let dt = 0.005; // 200Hz + if tilt.update_stationary(0.5, 0.3, dt) { + learned = true; + } + } + + // Should have learned by now - verify by checking correction works + assert!(learned, "Tilt should have been learned"); + + // If offset was learned as ~(0.5, 0.3), then correct(0.5, 0.3) should give ~(0, 0) + let (ax, ay) = tilt.correct(0.5, 0.3); + assert!(ax.abs() < 0.1, "Corrected X should be ~0: {}", ax); + assert!(ay.abs() < 0.1, "Corrected Y should be ~0: {}", ay); + } + + #[test] + fn test_tilt_correction() { + let mut tilt = TiltEstimator::new(0.5); + + // Learn offset + for _ in 0..200 { + tilt.update_stationary(0.4, -0.2, 0.005); + } + + // Apply correction + let (ax, ay) = tilt.correct(0.5, 0.0); + assert!((ax - 0.1).abs() < 0.15); // 0.5 - 0.4 = 0.1 + assert!((ay - 0.2).abs() < 0.15); // 0.0 - (-0.2) = 0.2 + } + + #[test] + fn test_gravity_estimator_steady_state() { + let config = FusionConfig { + gravity_learn_time: 0.5, + steady_state_speed_tolerance: 1.0, + steady_state_yaw_tolerance: 0.1, + gravity_alpha: 0.1, + ..Default::default() + }; + + let mut grav = GravityEstimator::new(&config); + + // Simulate steady-state driving at 15 m/s with small gravity offset + for _ in 0..200 { + grav.update( + 0.3, // ax offset + -0.2, // ay offset + 15.0, // speed + 0.01, // low yaw rate + 0.01, // dt + ); + } + + // Verify learning by checking correction reduces the offset + // If it learned offset ~(0.3, -0.2), then correct(0.3, -0.2) should give ~(0, 0) + let (ax, ay) = grav.correct(0.3, -0.2); + assert!(ax.abs() < 0.1, "Corrected X should be ~0: {}", ax); + assert!(ay.abs() < 0.1, "Corrected Y should be ~0: {}", ay); + } + + #[test] + fn test_sensor_fusion_blending() { + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Simulate high GPS rate + fusion.gps_accel.rate_ema = 25.0; + fusion.gps_accel.update(10.0, 0.0); + fusion.gps_accel.update(12.0, 0.04); // 50 m/s² GPS accel + + // Process IMU with different acceleration + let (lon, _lat) = fusion.process_imu( + 20.0, // IMU shows 20 m/s² in earth X + 0.0, // No Y accel + 0.0, // Heading East + 10.0, // Moving + 0.0, // No yaw rate + 0.005, + false, // Not stationary + ); + + // With 90% GPS weight, result should be closer to GPS + // GPS: 50 m/s², IMU: ~20 m/s² (after filtering) + // We can't predict exact value due to filter transient + assert!(lon.abs() < 100.0, "Longitudinal should be reasonable"); + } + + #[test] + fn test_earth_to_car_transform() { + // Heading East (yaw = 0) + let (lon, lat) = earth_to_car(10.0, 0.0, 0.0); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + + // Heading North (yaw = π/2) + let (lon, lat) = earth_to_car(0.0, 10.0, core::f32::consts::FRAC_PI_2); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + + // Heading West (yaw = π) + let (lon, lat) = earth_to_car(-10.0, 0.0, core::f32::consts::PI); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + } + + #[test] + fn test_gps_rate_calculation() { + let mut gps = GpsAcceleration::new(); + + // Initialize + gps.update_rate(0, 0.0); + + // After 1 second with 25 fixes + gps.update_rate(25, 1.0); + assert!((gps.get_rate() - 7.5).abs() < 1.0); // First update, EMA starts low + + // After another second with 25 more fixes + gps.update_rate(50, 2.0); + let rate = gps.get_rate(); + assert!(rate > 10.0 && rate < 30.0, "Rate should converge: {}", rate); + } +} diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 7af904f..9bd5be2 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -1,6 +1,8 @@ mod binary_telemetry; mod config; mod diagnostics; +mod filter; +mod fusion; mod gps; mod imu; mod mode; @@ -15,6 +17,7 @@ use std::sync::Arc; use config::{SystemConfig, WifiModeConfig}; use diagnostics::DiagnosticsState; +use fusion::{FusionConfig, SensorFusion}; use esp_idf_hal::{ delay::FreeRtos, peripherals::Peripherals, @@ -442,6 +445,37 @@ fn main() { let mut estimator = StateEstimator::new(); let mut publisher = TelemetryPublisher::new(udp_stream, mqtt_opt, telemetry_state.clone()); + // Create sensor fusion for hybrid acceleration (GPS + IMU blending) + // + // This provides: + // 1. Tilt correction - learns mounting offset when stopped (3s) + // 2. Gravity correction - learns while driving at steady speed + // 3. Vibration filtering - 2Hz Biquad removes engine/road noise + // 4. GPS/IMU blending - 70% GPS at 25Hz for balanced response + // + // Latency: ~80-100ms (vs ~50ms IMU-only, ~150ms with 90% GPS) + // Good for: city, highway, canyon. For track, increase GPS weights. + // + // NOTE: Filter runs at telemetry rate (~20Hz), not IMU rate (200Hz) + let telemetry_rate = 1000.0 / config.telemetry.interval_ms as f32; + let fusion_config = FusionConfig { + imu_sample_rate: telemetry_rate, + accel_filter_cutoff: 2.0, // 2Hz - passes driving, blocks vibration + gps_high_rate: 20.0, + gps_medium_rate: 10.0, + gps_max_age: 0.2, + // Balanced blend for city/highway/canyon (faster than 90% GPS) + gps_weight_high: 0.70, // 70% GPS / 30% IMU at 25Hz + gps_weight_medium: 0.50, // 50% / 50% at 10-20Hz + gps_weight_low: 0.30, // 30% GPS / 70% IMU fallback + tilt_learn_time: 3.0, + gravity_learn_time: 2.0, + steady_state_speed_tolerance: 0.5, + steady_state_yaw_tolerance: 0.087, + gravity_alpha: 0.02, + }; + let mut sensor_fusion = SensorFusion::new(fusion_config); + // Main loop timing let mut last_telemetry_ms = 0u32; let mut last_heap_check_ms = 0u32; @@ -715,10 +749,20 @@ fn main() { let speed = (vx * vx + vy * vy).sqrt(); estimator.update_velocity(vx, vy); estimator.update_speed(speed); + + // Feed GPS speed to sensor fusion for acceleration calculation + let time_s = now_ms as f32 / 1000.0; + sensor_fusion.process_gps(speed, time_s); } } } + // Update GPS rate estimate periodically (every second) + if now_ms % 1000 < 50 { + let fix_count = diagnostics.gps_fix_count(); + sensor_fusion.update_gps_rate(fix_count, now_ms as f32 / 1000.0); + } + // Update GPS status let gps_locked = sensors.gps_parser.is_warmed_up(); // Remove the .valid check status_mgr.update_gps_status( @@ -730,9 +774,13 @@ fn main() { // Publish telemetry at configured rate if now_ms - last_telemetry_ms >= config.telemetry.interval_ms { - // Update mode classifier + // Get current speed and check if stationary let speed = sensors.get_speed(Some(&estimator.ekf)); let (ax_corr, ay_corr, _) = sensors.imu_parser.get_accel_corrected(); + let is_stationary = + sensors.is_stationary(ax_corr, ay_corr, sensors.imu_parser.data().wz); + + // Transform IMU to earth frame for sensor fusion let (ax_b, ay_b, _) = remove_gravity( ax_corr, ay_corr, @@ -748,10 +796,23 @@ fn main() { sensors.imu_parser.data().pitch, estimator.ekf.yaw(), ); - estimator.update_mode( + + // Process through sensor fusion (GPS/IMU blending, filtering, tilt correction) + let dt = config.telemetry.interval_ms as f32 / 1000.0; + let (lon_blended, lat_filtered) = sensor_fusion.process_imu( ax_e, ay_e, estimator.ekf.yaw(), + speed, + sensors.imu_parser.data().wz, + dt, + is_stationary, + ); + + // Update mode classifier with hybrid (blended) acceleration + estimator.mode_classifier.update_hybrid( + lon_blended, + lat_filtered, sensors.imu_parser.data().wz, speed, ); diff --git a/sensors/blackbox/src/mode.rs b/sensors/blackbox/src/mode.rs index 2c08820..7abb0cd 100644 --- a/sensors/blackbox/src/mode.rs +++ b/sensors/blackbox/src/mode.rs @@ -3,6 +3,18 @@ /// Driving mode classifier /// Detects IDLE, ACCEL, BRAKE, CORNER, ACCEL+CORNER, and BRAKE+CORNER modes /// based on acceleration and yaw rate +/// +/// ## Hybrid Acceleration Support +/// +/// This classifier can operate in two modes: +/// 1. **Legacy mode**: Uses `update()` with raw earth-frame IMU accelerations +/// 2. **Hybrid mode**: Uses `update_hybrid()` with pre-blended longitudinal acceleration +/// +/// Hybrid mode receives longitudinal acceleration that's already blended from +/// GPS-derived (from velocity changes) and IMU sources. This provides: +/// - Drift-free longitudinal detection (GPS doesn't drift) +/// - Mounting angle independence (GPS measures actual vehicle acceleration) +/// - Graceful fallback when GPS is unavailable use sensor_fusion::transforms::earth_to_car; const G: f32 = 9.80665; // m/s² @@ -280,6 +292,80 @@ impl ModeClassifier { pub fn update_config(&mut self, config: ModeConfig) { self.config = config; } + + /// Update mode using pre-blended hybrid acceleration + /// + /// This is the preferred method when using SensorFusion, as it receives + /// longitudinal acceleration that's already blended from GPS and IMU sources. + /// + /// # Arguments + /// * `a_lon_blended` - Blended longitudinal acceleration (m/s², vehicle frame) + /// * `a_lat_filtered` - Filtered lateral acceleration (m/s², vehicle frame) + /// * `wz` - Yaw rate (rad/s) + /// * `speed` - Vehicle speed (m/s) + pub fn update_hybrid(&mut self, a_lon_blended: f32, a_lat_filtered: f32, wz: f32, speed: f32) { + // Update display speed with EMA + self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; + if self.v_disp < 3.0 / 3.6 { + self.v_disp = 0.0; + } + + // Convert to g units (blended accel is already in m/s²) + let a_lon = a_lon_blended / G; + let a_lat = a_lat_filtered / G; + + // Update EMA filters + // Note: longitudinal already filtered/blended, but still apply EMA for consistency + let alpha = self.config.alpha; + self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; + self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; + self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; + + // If stopped, clear all modes + if speed < self.config.min_speed { + self.mode.clear(); + return; + } + + // Check cornering (independent of accel/brake) + let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr + && self.yaw_ema.abs() > self.config.yaw_thr + && (self.a_lat_ema * self.yaw_ema) > 0.0; + + let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit + && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; + + if self.mode.has_corner() { + if cornering_exit { + self.mode.set_corner(false); + } + } else if cornering_active { + self.mode.set_corner(true); + } + + // Check acceleration (mutually exclusive with braking) + if self.mode.has_accel() { + if self.a_lon_ema < self.config.acc_exit { + self.mode.set_accel(false); + } + } else if !self.mode.has_brake() && self.a_lon_ema > self.config.acc_thr { + self.mode.set_accel(true); + } + + // Check braking (mutually exclusive with acceleration) + if self.mode.has_brake() { + if self.a_lon_ema > self.config.brake_exit { + self.mode.set_brake(false); + } + } else if !self.mode.has_accel() && self.a_lon_ema < self.config.brake_thr { + self.mode.set_brake(true); + } + } + + /// Get EMA filtered longitudinal acceleration (for diagnostics) + pub fn get_a_lon_ema(&self) -> f32 { + self.a_lon_ema + } } #[cfg(test)] From b80418837fd54efedcea6ef060857e25d782dd56 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 16:09:43 -0800 Subject: [PATCH 02/30] Send tilt-corrected accelerations to dashboard - Display corrected G-meter values instead of raw IMU data - Tilt learning now visible on dashboard (G-meter returns to zero) - Fix clippy warnings (doc comments, unused methods) --- sensors/blackbox/src/fusion.rs | 33 +++++++++++++++++++++++---------- sensors/blackbox/src/main.rs | 4 ++-- sensors/blackbox/src/system.rs | 20 ++++++++++++++------ 3 files changed, 39 insertions(+), 18 deletions(-) diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs index 76b0bb5..a165b8a 100644 --- a/sensors/blackbox/src/fusion.rs +++ b/sensors/blackbox/src/fusion.rs @@ -1,13 +1,13 @@ -/// Sensor Fusion Module -/// -/// This module handles: -/// 1. GPS-derived longitudinal acceleration (from velocity changes) -/// 2. GPS/IMU acceleration blending with adaptive weights -/// 3. Dynamic tilt offset learning (ZUPT enhancement) -/// 4. Moving gravity estimation (continuous calibration while driving) -/// -/// The goal is to provide clean, drift-free acceleration for mode detection -/// that works regardless of device mounting angle. +//! Sensor Fusion Module +//! +//! This module handles: +//! 1. GPS-derived longitudinal acceleration (from velocity changes) +//! 2. GPS/IMU acceleration blending with adaptive weights +//! 3. Dynamic tilt offset learning (ZUPT enhancement) +//! 4. Moving gravity estimation (continuous calibration while driving) +//! +//! The goal is to provide clean, drift-free acceleration for mode detection +//! that works regardless of device mounting angle. use crate::filter::BiquadFilter; @@ -452,6 +452,7 @@ impl SensorFusion { /// /// # Returns /// (lon_accel, lat_accel) - Fused accelerations in vehicle frame (m/s²) + #[allow(clippy::too_many_arguments)] pub fn process_imu( &mut self, ax_earth: f32, @@ -534,6 +535,18 @@ impl SensorFusion { self.gps_accel.update_rate(fix_count, time); } + /// Get filtered longitudinal acceleration (vehicle frame, m/s²) + /// Positive = forward acceleration + pub fn get_lon_filtered(&self) -> f32 { + self.imu_lon_filtered + } + + /// Get filtered lateral acceleration (vehicle frame, m/s²) + /// Positive = left + pub fn get_lat_filtered(&self) -> f32 { + self.imu_lat_filtered + } + /// Compute GPS weight based on GPS rate and data freshness fn compute_gps_weight(&self) -> f32 { let rate = self.gps_accel.get_rate(); diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 9bd5be2..46d071c 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -817,9 +817,9 @@ fn main() { speed, ); - // Publish telemetry + // Publish telemetry with tilt-corrected accelerations publisher - .publish_telemetry(&sensors, &estimator, now_ms) + .publish_telemetry(&sensors, &estimator, &sensor_fusion, now_ms) .ok(); last_telemetry_ms = now_ms; } diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index 2fc057b..974100a 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -336,18 +336,26 @@ impl TelemetryPublisher { &mut self, sensors: &SensorManager, estimator: &StateEstimator, + sensor_fusion: &crate::fusion::SensorFusion, now_ms: u32, ) -> Result<(), SystemError> { - // Send bias-corrected accelerations (client can process further if needed) - let (ax_corr, ay_corr, az_corr) = sensors.imu_parser.get_accel_corrected(); + // Get tilt/gravity-corrected, filtered accelerations in vehicle frame + // These are clean values suitable for G-meter display + let lon_filt = sensor_fusion.get_lon_filtered(); + let lat_filt = sensor_fusion.get_lat_filtered(); + + // Keep raw az for reference (no tilt correction needed for vertical) + let (_, _, az_corr) = sensors.imu_parser.get_accel_corrected(); let mut packet = binary_telemetry::TelemetryPacket::new(); packet.timestamp_ms = now_ms; - // Send bias-corrected accelerations in body frame - // Client-side processing can remove gravity and transform to vehicle frame - packet.ax = ax_corr; - packet.ay = ay_corr; + // Send corrected vehicle-frame accelerations + // Dashboard expects: lng = -ax/9.81, latg = ay/9.81 + // So: ax = -lon (negated so dashboard shows positive for accel) + // ay = lat (positive = right turn on G-meter) + packet.ax = -lon_filt; + packet.ay = -lat_filt; // Negate: fusion uses left-positive, dashboard uses right-positive packet.az = az_corr; packet.wz = sensors.imu_parser.data().wz; packet.roll = sensors.imu_parser.data().roll.to_radians(); From 508767517c0f959ec8162b6e8cc861e5b94eb58c Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 16:15:39 -0800 Subject: [PATCH 03/30] Fix display/mode mismatch and update documentation - Dashboard now shows blended longitudinal acceleration (same as mode classifier) - Add unit tests verifying display values match classification inputs - Fix CLAUDE.md blending ratios (70/50/30 not 90/70/30) - Update README.md with noise cleanup features - Add data flow diagram showing dashboard receives same values as mode.rs Co-Authored-By: Claude --- CLAUDE.md | 18 +++++-- README.md | 5 +- sensors/blackbox/src/fusion.rs | 93 ++++++++++++++++++++++++++++++++-- sensors/blackbox/src/system.rs | 9 ++-- 4 files changed, 113 insertions(+), 12 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 7a689a5..1a3c0c9 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -605,14 +605,17 @@ Handles GPS/IMU blending, tilt correction, and continuous calibration. - Low-pass filters to remove vibration - Blends GPS and IMU for longitudinal acceleration -**GPS/IMU Blending Ratios:** +**GPS/IMU Blending Ratios (configurable in FusionConfig):** ``` -GPS rate >= 20Hz, fresh: 90% GPS / 10% IMU (high confidence) -GPS rate 10-20Hz: 70% GPS / 30% IMU (medium confidence) +GPS rate >= 20Hz, fresh: 70% GPS / 30% IMU (high confidence) +GPS rate 10-20Hz: 50% GPS / 50% IMU (medium confidence) GPS rate < 10Hz: 30% GPS / 70% IMU (low confidence) GPS stale (>200ms): 0% GPS / 100% IMU (fallback) ``` +These ratios balance responsiveness (~80-100ms latency) with accuracy. +For track use, consider increasing GPS weights for maximum accuracy. + **Data Flow:** ``` GPS (25Hz) → GpsAcceleration → lon_accel_gps @@ -621,9 +624,16 @@ IMU → remove_gravity → body_to_earth → TiltCorrect → GravityCorrect → ↓ lon_accel_imu ↓ - Blend(gps, imu) → mode.rs + Blend(gps, imu) → lon_blended + ↓ + ┌───────────────────────┴───────────────────────┐ + ↓ ↓ + mode.rs (classification) Dashboard (G-meter) ``` +**Dashboard Display:** The G-meter shows the same blended longitudinal and filtered lateral +values that mode classification uses. What you see is what the algorithm sees. + ### websocket_server.rs - HTTP Dashboard Server **Overview:** diff --git a/README.md b/README.md index 62d68f2..204cb90 100644 --- a/README.md +++ b/README.md @@ -21,11 +21,14 @@ A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's - **Built-in mobile dashboard** - view live data on your phone **How it works:** -- 7-state Extended Kalman Filter fuses GPS (5Hz) and IMU (50Hz) +- 7-state Extended Kalman Filter fuses GPS (5-25Hz) and IMU (200Hz) - Gyro integrates yaw between GPS updates to maintain heading - Body-frame accelerations transformed to earth frame using current orientation - Zero-velocity updates eliminate IMU drift when stationary - Constant Turn Rate and Acceleration (CTRA) model for cornering +- **GPS/IMU blending** for accurate mode detection independent of mounting angle +- **Automatic tilt correction** learns device mounting offset when stopped +- **Vibration filtering** removes engine/road noise (2Hz low-pass Butterworth) **Why it's useful:** - Track day data logging without $1000+ commercial systems diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs index a165b8a..9711265 100644 --- a/sensors/blackbox/src/fusion.rs +++ b/sensors/blackbox/src/fusion.rs @@ -535,13 +535,16 @@ impl SensorFusion { self.gps_accel.update_rate(fix_count, time); } - /// Get filtered longitudinal acceleration (vehicle frame, m/s²) + /// Get blended longitudinal acceleration (vehicle frame, m/s²) + /// This is GPS/IMU blended - same value used by mode classification /// Positive = forward acceleration - pub fn get_lon_filtered(&self) -> f32 { - self.imu_lon_filtered + pub fn get_lon_blended(&self) -> f32 { + self.blended_lon } /// Get filtered lateral acceleration (vehicle frame, m/s²) + /// Pure IMU with tilt/gravity correction and low-pass filter + /// Same value used by mode classification (GPS can't sense lateral) /// Positive = left pub fn get_lat_filtered(&self) -> f32 { self.imu_lat_filtered @@ -769,4 +772,88 @@ mod tests { let rate = gps.get_rate(); assert!(rate > 10.0 && rate < 30.0, "Rate should converge: {}", rate); } + + #[test] + fn test_display_matches_mode_classification() { + // Critical test: verify that get_lon_blended() and get_lat_filtered() + // return the exact same values that process_imu() returns. + // This ensures dashboard displays what mode classification sees. + + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Setup GPS rate for blending + fusion.gps_accel.rate_ema = 25.0; + fusion.gps_accel.update(10.0, 0.0); + fusion.gps_accel.update(12.0, 0.04); // GPS shows acceleration + + // Process IMU + let (lon_from_process, lat_from_process) = fusion.process_imu( + 5.0, // IMU earth X + 3.0, // IMU earth Y + 0.0, // Heading East + 10.0, // Moving + 0.0, // No yaw rate + 0.05, // dt + false, // Not stationary + ); + + // Get values that would be sent to dashboard + let lon_for_display = fusion.get_lon_blended(); + let lat_for_display = fusion.get_lat_filtered(); + + // These MUST match exactly - this is what the test is verifying + assert_eq!( + lon_from_process, lon_for_display, + "Dashboard lon must match mode classifier input" + ); + assert_eq!( + lat_from_process, lat_for_display, + "Dashboard lat must match mode classifier input" + ); + } + + #[test] + fn test_blending_affects_display() { + // Verify GPS/IMU blending actually works and affects the displayed value + let config = FusionConfig { + gps_weight_high: 0.70, // 70% GPS + ..Default::default() + }; + let mut fusion = SensorFusion::new(config); + + // Setup high GPS rate (25Hz) for maximum GPS weight + fusion.gps_accel.rate_ema = 25.0; + + // GPS shows 0 acceleration (constant speed) + fusion.gps_accel.update(10.0, 0.0); + fusion.gps_accel.update(10.0, 0.04); // Same speed = 0 accel + + // IMU shows 5 m/s² forward acceleration + // With 70% GPS (0) and 30% IMU (~5), blended should be ~1.5 m/s² + // (exact value depends on filter state) + let (lon, _lat) = fusion.process_imu( + 5.0, // IMU earth X (forward) + 0.0, // No lateral + 0.0, // Heading East + 10.0, // Moving + 0.0, // No yaw rate + 0.05, // dt + false, // Not stationary + ); + + // The blended value should be less than pure IMU (5.0) + // because GPS (showing 0) pulls it down + let display_lon = fusion.get_lon_blended(); + assert_eq!(lon, display_lon, "Display must match process output"); + + // With 70% GPS weight showing 0, the blended value should be + // significantly less than the IMU value of 5.0 + // After filter settling, expect roughly: 0.70 * 0 + 0.30 * IMU_filtered + assert!( + display_lon.abs() < 4.0, + "GPS blending should reduce value: got {}", + display_lon + ); + } } diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index 974100a..a128e1e 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -339,9 +339,10 @@ impl TelemetryPublisher { sensor_fusion: &crate::fusion::SensorFusion, now_ms: u32, ) -> Result<(), SystemError> { - // Get tilt/gravity-corrected, filtered accelerations in vehicle frame - // These are clean values suitable for G-meter display - let lon_filt = sensor_fusion.get_lon_filtered(); + // Get accelerations matching what mode classification sees + // lon: GPS/IMU blended (same as mode classifier input) + // lat: filtered IMU (same as mode classifier input) + let lon_blended = sensor_fusion.get_lon_blended(); let lat_filt = sensor_fusion.get_lat_filtered(); // Keep raw az for reference (no tilt correction needed for vertical) @@ -354,7 +355,7 @@ impl TelemetryPublisher { // Dashboard expects: lng = -ax/9.81, latg = ay/9.81 // So: ax = -lon (negated so dashboard shows positive for accel) // ay = lat (positive = right turn on G-meter) - packet.ax = -lon_filt; + packet.ax = -lon_blended; packet.ay = -lat_filt; // Negate: fusion uses left-positive, dashboard uses right-positive packet.az = az_corr; packet.wz = sensors.imu_parser.data().wz; From 880d39e05838e52accfc4dd6233d2c5280e91957 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 17:48:35 -0800 Subject: [PATCH 04/30] Fix lateral G sticking after turns - use centripetal formula Root cause: Filtering lateral acceleration in earth frame before transforming to vehicle frame caused the filter to smooth a rotating vector. When the vehicle yaws, the filtered earth-frame values get transformed with the new heading, causing lateral G to "stick" at incorrect values until the vehicle stops. Changes: - Remove Biquad filter for lateral in earth frame entirely - Use centripetal formula (speed * yaw_rate) for mode detection This is what pro racing systems (VBOX, Motec) use - mount-independent - Filter longitudinal in vehicle frame (safe, no rotation issues) - Increase filter cutoff from 2Hz to 5Hz for faster response - Rename config fields: imu_sample_rate -> sample_rate, accel_filter_cutoff -> lon_filter_cutoff Display continues to show accelerometer-based lateral with tilt/gravity correction. Mode detection uses centripetal lateral which responds immediately when yaw rate changes. Added smart unit tests that simulate turn scenarios and verify lateral returns to zero immediately when turn ends. --- sensors/blackbox/src/fusion.rs | 316 ++++++++++++++------------------- sensors/blackbox/src/main.rs | 12 +- sensors/blackbox/src/system.rs | 4 +- 3 files changed, 141 insertions(+), 191 deletions(-) diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs index 9711265..2e7fb7f 100644 --- a/sensors/blackbox/src/fusion.rs +++ b/sensors/blackbox/src/fusion.rs @@ -38,10 +38,11 @@ pub struct FusionConfig { pub gravity_learn_time: f32, /// Alpha for gravity estimate update (smaller = slower, more stable) pub gravity_alpha: f32, - /// Low-pass filter cutoff for acceleration (Hz) - pub accel_filter_cutoff: f32, - /// IMU sample rate (Hz) - pub imu_sample_rate: f32, + /// Low-pass filter cutoff for longitudinal acceleration (Hz) + /// Only applied in vehicle frame for longitudinal + pub lon_filter_cutoff: f32, + /// Sample rate for telemetry (Hz) + pub sample_rate: f32, } impl Default for FusionConfig { @@ -68,8 +69,8 @@ impl Default for FusionConfig { steady_state_yaw_tolerance: 0.087, // ~5 deg/s gravity_learn_time: 2.0, gravity_alpha: 0.02, // Very slow update - accel_filter_cutoff: 4.0, - imu_sample_rate: 200.0, + lon_filter_cutoff: 5.0, // 5Hz for longitudinal (faster than before) + sample_rate: 20.0, // Telemetry rate } } } @@ -408,16 +409,14 @@ pub struct SensorFusion { pub tilt_estimator: TiltEstimator, /// Gravity estimator (learns while driving) pub gravity_estimator: GravityEstimator, - /// Low-pass filter for IMU X - filter_x: BiquadFilter, - /// Low-pass filter for IMU Y - filter_y: BiquadFilter, - /// Filtered IMU longitudinal acceleration (m/s²) - imu_lon_filtered: f32, - /// Filtered IMU lateral acceleration (m/s²) - imu_lat_filtered: f32, + /// Low-pass filter for longitudinal IMU (applied in vehicle frame) + filter_lon: BiquadFilter, /// Blended longitudinal acceleration (m/s²) blended_lon: f32, + /// Corrected lateral acceleration for display (m/s², vehicle frame) + lat_corrected: f32, + /// Centripetal lateral acceleration for mode detection (m/s²) + lat_centripetal: f32, /// Last GPS blend weight (for diagnostics) last_gps_weight: f32, /// Was stationary last update? @@ -430,28 +429,31 @@ impl SensorFusion { gps_accel: GpsAcceleration::new(), tilt_estimator: TiltEstimator::new(config.tilt_learn_time), gravity_estimator: GravityEstimator::new(&config), - filter_x: BiquadFilter::new_lowpass(config.accel_filter_cutoff, config.imu_sample_rate), - filter_y: BiquadFilter::new_lowpass(config.accel_filter_cutoff, config.imu_sample_rate), - imu_lon_filtered: 0.0, - imu_lat_filtered: 0.0, + filter_lon: BiquadFilter::new_lowpass(config.lon_filter_cutoff, config.sample_rate), blended_lon: 0.0, + lat_corrected: 0.0, + lat_centripetal: 0.0, last_gps_weight: 0.0, was_stationary: false, config, } } - /// Process IMU data (call at IMU rate, e.g., 200Hz) + /// Process IMU data (call at telemetry rate, e.g., 20Hz) /// /// # Arguments /// * `ax_earth` - X acceleration in earth frame (m/s²) /// * `ay_earth` - Y acceleration in earth frame (m/s²) /// * `yaw` - Vehicle yaw angle (rad, for earth-to-car transform) + /// * `speed` - Vehicle speed (m/s) + /// * `yaw_rate` - Yaw rate (rad/s) /// * `dt` - Time step (seconds) /// * `is_stationary` - Whether vehicle is currently stationary /// /// # Returns - /// (lon_accel, lat_accel) - Fused accelerations in vehicle frame (m/s²) + /// (lon_blended, lat_centripetal) - Accelerations for mode detection (m/s²) + /// lon_blended: GPS/IMU blended longitudinal + /// lat_centripetal: speed * yaw_rate (pro-style, mount-independent) #[allow(clippy::too_many_arguments)] pub fn process_imu( &mut self, @@ -470,17 +472,20 @@ impl SensorFusion { let (ax_tilt, ay_tilt) = self.tilt_estimator.correct(ax_earth, ay_earth); // Apply gravity correction (learned while driving) - let (ax_grav, ay_grav) = self.gravity_estimator.correct(ax_tilt, ay_tilt); + let (ax_corr, ay_corr) = self.gravity_estimator.correct(ax_tilt, ay_tilt); - // Low-pass filter to remove vibration - let ax_filt = self.filter_x.process(ax_grav); - let ay_filt = self.filter_y.process(ay_grav); + // Transform to vehicle frame (NO filtering in earth frame!) + let (lon_imu_raw, lat_imu_raw) = earth_to_car(ax_corr, ay_corr, yaw); - // Transform to vehicle frame - let (lon_imu, lat_imu) = earth_to_car(ax_filt, ay_filt, yaw); + // Filter longitudinal in vehicle frame (safe, no rotation issues) + let lon_imu = self.filter_lon.process(lon_imu_raw); - self.imu_lon_filtered = lon_imu; - self.imu_lat_filtered = lat_imu; + // Store corrected lateral for display (no Biquad filter - mode.rs EMA handles it) + self.lat_corrected = lat_imu_raw; + + // Calculate centripetal lateral for mode detection (pro-style) + // a_lateral = v * omega (mount-angle independent, no filtering needed) + self.lat_centripetal = speed * yaw_rate; // Handle stationary state transitions if is_stationary { @@ -489,9 +494,8 @@ impl SensorFusion { .update_stationary(ax_earth, ay_earth, dt); if !self.was_stationary { - // Just stopped - reset filters to avoid transient - self.filter_x.reset_to(0.0); - self.filter_y.reset_to(0.0); + // Just stopped - reset filter to avoid transient + self.filter_lon.reset_to(0.0); } } else { if self.was_stationary { @@ -517,8 +521,8 @@ impl SensorFusion { self.blended_lon = lon_blended; - // Lateral always from IMU (GPS can't sense lateral well) - (lon_blended, lat_imu) + // Return blended longitudinal and centripetal lateral for mode detection + (lon_blended, self.lat_centripetal) } /// Process GPS speed update (call at GPS rate, e.g., 25Hz) @@ -542,12 +546,18 @@ impl SensorFusion { self.blended_lon } - /// Get filtered lateral acceleration (vehicle frame, m/s²) - /// Pure IMU with tilt/gravity correction and low-pass filter - /// Same value used by mode classification (GPS can't sense lateral) + /// Get corrected lateral acceleration for display (vehicle frame, m/s²) + /// This is accelerometer-based with tilt/gravity correction /// Positive = left - pub fn get_lat_filtered(&self) -> f32 { - self.imu_lat_filtered + pub fn get_lat_display(&self) -> f32 { + self.lat_corrected + } + + /// Get centripetal lateral acceleration for mode detection (m/s²) + /// This is speed * yaw_rate (pro-style, mount-independent) + #[allow(dead_code)] + pub fn get_lat_centripetal(&self) -> f32 { + self.lat_centripetal } /// Compute GPS weight based on GPS rate and data freshness @@ -645,97 +655,121 @@ mod tests { } #[test] - fn test_tilt_estimator_learns() { + fn test_tilt_estimator_learns_and_corrects() { let mut tilt = TiltEstimator::new(1.0); // 1 second learn time - // Simulate stationary with 0.5 m/s² offset - let mut learned = false; + // Simulate stationary with 0.5 m/s² offset (tilted mount) for _ in 0..300 { - let dt = 0.005; // 200Hz - if tilt.update_stationary(0.5, 0.3, dt) { - learned = true; - } + // 1.5 seconds at 200Hz + tilt.update_stationary(0.5, 0.3, 0.005); } - // Should have learned by now - verify by checking correction works - assert!(learned, "Tilt should have been learned"); - - // If offset was learned as ~(0.5, 0.3), then correct(0.5, 0.3) should give ~(0, 0) + // Correction should now reduce the offset to near zero let (ax, ay) = tilt.correct(0.5, 0.3); assert!(ax.abs() < 0.1, "Corrected X should be ~0: {}", ax); assert!(ay.abs() < 0.1, "Corrected Y should be ~0: {}", ay); } #[test] - fn test_tilt_correction() { - let mut tilt = TiltEstimator::new(0.5); + fn test_centripetal_lateral_during_turn() { + // Simulate a turn: speed = 10 m/s, yaw_rate = 0.3 rad/s + // Expected centripetal: 10 * 0.3 = 3 m/s² + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); - // Learn offset - for _ in 0..200 { - tilt.update_stationary(0.4, -0.2, 0.005); - } + // Process with turn conditions + let (_lon, lat) = fusion.process_imu( + 0.0, // No earth-frame X accel + 0.0, // No earth-frame Y accel + 0.0, // Heading + 10.0, // Speed = 10 m/s + 0.3, // Yaw rate = 0.3 rad/s (turning) + 0.05, // dt + false, // Not stationary + ); - // Apply correction - let (ax, ay) = tilt.correct(0.5, 0.0); - assert!((ax - 0.1).abs() < 0.15); // 0.5 - 0.4 = 0.1 - assert!((ay - 0.2).abs() < 0.15); // 0.0 - (-0.2) = 0.2 + // Lateral should be centripetal: speed * yaw_rate = 3 m/s² + assert!( + (lat - 3.0).abs() < 0.01, + "Centripetal lateral should be 3.0: got {}", + lat + ); } #[test] - fn test_gravity_estimator_steady_state() { - let config = FusionConfig { - gravity_learn_time: 0.5, - steady_state_speed_tolerance: 1.0, - steady_state_yaw_tolerance: 0.1, - gravity_alpha: 0.1, - ..Default::default() - }; + fn test_lateral_returns_to_zero_after_turn() { + // This test verifies the critical bug fix: + // After a turn ends, lateral should return to zero immediately + // (not "stick" due to earth-frame filtering) - let mut grav = GravityEstimator::new(&config); + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); - // Simulate steady-state driving at 15 m/s with small gravity offset - for _ in 0..200 { - grav.update( - 0.3, // ax offset - -0.2, // ay offset - 15.0, // speed - 0.01, // low yaw rate - 0.01, // dt + // Simulate a 90° turn + let mut yaw = 0.0; + for i in 0..20 { + // 1 second of turning at 20Hz + let yaw_rate = 1.57; // ~90°/s + yaw += yaw_rate * 0.05; + + fusion.process_imu( + 0.0, // Earth X + 0.0, // Earth Y + yaw, // Changing heading + 10.0, // Constant speed + yaw_rate, // Turning + 0.05, // dt + false, // Not stationary ); + + // During turn, lateral should be positive (speed * positive yaw_rate) + if i > 5 { + assert!( + fusion.get_lat_centripetal() > 10.0, + "During turn, lateral should be high: {}", + fusion.get_lat_centripetal() + ); + } } - // Verify learning by checking correction reduces the offset - // If it learned offset ~(0.3, -0.2), then correct(0.3, -0.2) should give ~(0, 0) - let (ax, ay) = grav.correct(0.3, -0.2); - assert!(ax.abs() < 0.1, "Corrected X should be ~0: {}", ax); - assert!(ay.abs() < 0.1, "Corrected Y should be ~0: {}", ay); + // Turn ends - yaw_rate goes to zero + let (_lon, lat) = fusion.process_imu( + 0.0, // Earth X + 0.0, // Earth Y + yaw, // Final heading + 10.0, // Still moving + 0.0, // NO MORE TURNING + 0.05, // dt + false, + ); + + // Lateral should be zero immediately (no sticking!) + assert!( + lat.abs() < 0.01, + "After turn ends, lateral should be ~0 immediately: got {}", + lat + ); } #[test] - fn test_sensor_fusion_blending() { + fn test_display_matches_mode_classification_longitudinal() { + // Verify that get_lon_blended() returns the same value as process_imu() let config = FusionConfig::default(); let mut fusion = SensorFusion::new(config); - // Simulate high GPS rate + // Setup GPS fusion.gps_accel.rate_ema = 25.0; fusion.gps_accel.update(10.0, 0.0); - fusion.gps_accel.update(12.0, 0.04); // 50 m/s² GPS accel - - // Process IMU with different acceleration - let (lon, _lat) = fusion.process_imu( - 20.0, // IMU shows 20 m/s² in earth X - 0.0, // No Y accel - 0.0, // Heading East - 10.0, // Moving - 0.0, // No yaw rate - 0.005, - false, // Not stationary - ); + fusion.gps_accel.update(12.0, 0.04); - // With 90% GPS weight, result should be closer to GPS - // GPS: 50 m/s², IMU: ~20 m/s² (after filtering) - // We can't predict exact value due to filter transient - assert!(lon.abs() < 100.0, "Longitudinal should be reasonable"); + let (lon_from_process, _lat) = fusion.process_imu(5.0, 3.0, 0.0, 10.0, 0.0, 0.05, false); + + let lon_for_display = fusion.get_lon_blended(); + + assert_eq!( + lon_from_process, lon_for_display, + "Display lon must match mode classifier input" + ); } #[test] @@ -772,88 +806,4 @@ mod tests { let rate = gps.get_rate(); assert!(rate > 10.0 && rate < 30.0, "Rate should converge: {}", rate); } - - #[test] - fn test_display_matches_mode_classification() { - // Critical test: verify that get_lon_blended() and get_lat_filtered() - // return the exact same values that process_imu() returns. - // This ensures dashboard displays what mode classification sees. - - let config = FusionConfig::default(); - let mut fusion = SensorFusion::new(config); - - // Setup GPS rate for blending - fusion.gps_accel.rate_ema = 25.0; - fusion.gps_accel.update(10.0, 0.0); - fusion.gps_accel.update(12.0, 0.04); // GPS shows acceleration - - // Process IMU - let (lon_from_process, lat_from_process) = fusion.process_imu( - 5.0, // IMU earth X - 3.0, // IMU earth Y - 0.0, // Heading East - 10.0, // Moving - 0.0, // No yaw rate - 0.05, // dt - false, // Not stationary - ); - - // Get values that would be sent to dashboard - let lon_for_display = fusion.get_lon_blended(); - let lat_for_display = fusion.get_lat_filtered(); - - // These MUST match exactly - this is what the test is verifying - assert_eq!( - lon_from_process, lon_for_display, - "Dashboard lon must match mode classifier input" - ); - assert_eq!( - lat_from_process, lat_for_display, - "Dashboard lat must match mode classifier input" - ); - } - - #[test] - fn test_blending_affects_display() { - // Verify GPS/IMU blending actually works and affects the displayed value - let config = FusionConfig { - gps_weight_high: 0.70, // 70% GPS - ..Default::default() - }; - let mut fusion = SensorFusion::new(config); - - // Setup high GPS rate (25Hz) for maximum GPS weight - fusion.gps_accel.rate_ema = 25.0; - - // GPS shows 0 acceleration (constant speed) - fusion.gps_accel.update(10.0, 0.0); - fusion.gps_accel.update(10.0, 0.04); // Same speed = 0 accel - - // IMU shows 5 m/s² forward acceleration - // With 70% GPS (0) and 30% IMU (~5), blended should be ~1.5 m/s² - // (exact value depends on filter state) - let (lon, _lat) = fusion.process_imu( - 5.0, // IMU earth X (forward) - 0.0, // No lateral - 0.0, // Heading East - 10.0, // Moving - 0.0, // No yaw rate - 0.05, // dt - false, // Not stationary - ); - - // The blended value should be less than pure IMU (5.0) - // because GPS (showing 0) pulls it down - let display_lon = fusion.get_lon_blended(); - assert_eq!(lon, display_lon, "Display must match process output"); - - // With 70% GPS weight showing 0, the blended value should be - // significantly less than the IMU value of 5.0 - // After filter settling, expect roughly: 0.70 * 0 + 0.30 * IMU_filtered - assert!( - display_lon.abs() < 4.0, - "GPS blending should reduce value: got {}", - display_lon - ); - } } diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 46d071c..9e27a97 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -459,15 +459,15 @@ fn main() { // NOTE: Filter runs at telemetry rate (~20Hz), not IMU rate (200Hz) let telemetry_rate = 1000.0 / config.telemetry.interval_ms as f32; let fusion_config = FusionConfig { - imu_sample_rate: telemetry_rate, - accel_filter_cutoff: 2.0, // 2Hz - passes driving, blocks vibration + sample_rate: telemetry_rate, + lon_filter_cutoff: 5.0, // 5Hz - faster response for longitudinal gps_high_rate: 20.0, gps_medium_rate: 10.0, gps_max_age: 0.2, - // Balanced blend for city/highway/canyon (faster than 90% GPS) - gps_weight_high: 0.70, // 70% GPS / 30% IMU at 25Hz - gps_weight_medium: 0.50, // 50% / 50% at 10-20Hz - gps_weight_low: 0.30, // 30% GPS / 70% IMU fallback + // Balanced blend for city/highway/canyon + gps_weight_high: 0.70, // 70% GPS / 30% IMU at 25Hz + gps_weight_medium: 0.50, // 50% / 50% at 10-20Hz + gps_weight_low: 0.30, // 30% GPS / 70% IMU fallback tilt_learn_time: 3.0, gravity_learn_time: 2.0, steady_state_speed_tolerance: 0.5, diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index a128e1e..08ce688 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -343,7 +343,7 @@ impl TelemetryPublisher { // lon: GPS/IMU blended (same as mode classifier input) // lat: filtered IMU (same as mode classifier input) let lon_blended = sensor_fusion.get_lon_blended(); - let lat_filt = sensor_fusion.get_lat_filtered(); + let lat_display = sensor_fusion.get_lat_display(); // Keep raw az for reference (no tilt correction needed for vertical) let (_, _, az_corr) = sensors.imu_parser.get_accel_corrected(); @@ -356,7 +356,7 @@ impl TelemetryPublisher { // So: ax = -lon (negated so dashboard shows positive for accel) // ay = lat (positive = right turn on G-meter) packet.ax = -lon_blended; - packet.ay = -lat_filt; // Negate: fusion uses left-positive, dashboard uses right-positive + packet.ay = -lat_display; // Negate: fusion uses left-positive, dashboard uses right-positive packet.az = az_corr; packet.wz = sensors.imu_parser.data().wz; packet.roll = sensors.imu_parser.data().roll.to_radians(); From ec8d7159d97e5525defb4e262dbc0eaf745f43d2 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 17:56:10 -0800 Subject: [PATCH 05/30] Add rule forbidding #[allow(dead_code)] and fix fusion.rs Added rule to CLAUDE.md: dead code must be removed or used, not silenced. Fixed fusion.rs: removed get_lat_centripetal() getter, updated test to use return value from process_imu() instead. --- CLAUDE.md | 1 + sensors/blackbox/src/fusion.rs | 13 +++---------- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 1a3c0c9..68289f5 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -27,6 +27,7 @@ You are a seasoned embedded Rust engineer with deep expertise in: - Efficient binary protocols (fixed-size packets, checksums) - Avoid over-engineering: solve the problem at hand, not hypothetical future problems - Keep abstractions minimal until they prove necessary +- **Never use `#[allow(dead_code)]`** - dead code must be removed or used, not silenced **Testing Philosophy:** - Write meaningful tests that catch real bugs, not boilerplate diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs index 2e7fb7f..22d0a7f 100644 --- a/sensors/blackbox/src/fusion.rs +++ b/sensors/blackbox/src/fusion.rs @@ -553,13 +553,6 @@ impl SensorFusion { self.lat_corrected } - /// Get centripetal lateral acceleration for mode detection (m/s²) - /// This is speed * yaw_rate (pro-style, mount-independent) - #[allow(dead_code)] - pub fn get_lat_centripetal(&self) -> f32 { - self.lat_centripetal - } - /// Compute GPS weight based on GPS rate and data freshness fn compute_gps_weight(&self) -> f32 { let rate = self.gps_accel.get_rate(); @@ -712,7 +705,7 @@ mod tests { let yaw_rate = 1.57; // ~90°/s yaw += yaw_rate * 0.05; - fusion.process_imu( + let (_lon, lat) = fusion.process_imu( 0.0, // Earth X 0.0, // Earth Y yaw, // Changing heading @@ -725,9 +718,9 @@ mod tests { // During turn, lateral should be positive (speed * positive yaw_rate) if i > 5 { assert!( - fusion.get_lat_centripetal() > 10.0, + lat > 10.0, "During turn, lateral should be high: {}", - fusion.get_lat_centripetal() + lat ); } } From 2329eb47aa61ce960c87b131ab49f5a2cccd5ebf Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 18:02:53 -0800 Subject: [PATCH 06/30] Update documentation and tests to match 5Hz filter config - README: Update filter description from 2Hz to 5Hz - mode.rs: Fix update_hybrid docs - receives centripetal lateral, not filtered - filter.rs: Add test for actual 5Hz config used in fusion.rs --- README.md | 2 +- sensors/blackbox/src/filter.rs | 81 +++++++++++++++++++++++++++++----- sensors/blackbox/src/mode.rs | 11 ++--- 3 files changed, 78 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index 204cb90..56d1b7e 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,7 @@ A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's - Constant Turn Rate and Acceleration (CTRA) model for cornering - **GPS/IMU blending** for accurate mode detection independent of mounting angle - **Automatic tilt correction** learns device mounting offset when stopped -- **Vibration filtering** removes engine/road noise (2Hz low-pass Butterworth) +- **Vibration filtering** removes engine/road noise (5Hz low-pass Butterworth for longitudinal) **Why it's useful:** - Track day data logging without $1000+ commercial systems diff --git a/sensors/blackbox/src/filter.rs b/sensors/blackbox/src/filter.rs index 5882c65..aba8e7d 100644 --- a/sensors/blackbox/src/filter.rs +++ b/sensors/blackbox/src/filter.rs @@ -112,9 +112,10 @@ mod tests { const SAMPLE_RATE: f32 = 200.0; const CUTOFF: f32 = 4.0; - // Also test with telemetry-rate parameters (20Hz sampling, 2Hz cutoff) + // Test with telemetry-rate parameters (20Hz sampling) const TELEM_RATE: f32 = 20.0; - const TELEM_CUTOFF: f32 = 2.0; + const TELEM_CUTOFF_SLOW: f32 = 2.0; // Conservative cutoff for testing + const TELEM_CUTOFF_FAST: f32 = 5.0; // Actual config used in fusion.rs #[test] fn test_filter_creation() { @@ -297,10 +298,9 @@ mod tests { } #[test] - fn test_telemetry_rate_filter() { - // Test at telemetry rate (20Hz sampling, 2Hz cutoff) - // This is the actual configuration used in main.rs - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF, TELEM_RATE); + fn test_telemetry_rate_filter_conservative() { + // Test at telemetry rate with conservative 2Hz cutoff + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_SLOW, TELEM_RATE); // DC should pass through for _ in 0..50 { @@ -313,13 +313,12 @@ mod tests { dc_output ); - // Reset and test low frequency (0.5 Hz driving input) - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF, TELEM_RATE); + // Test low frequency (0.5 Hz driving input) + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_SLOW, TELEM_RATE); let freq = 0.5; // 0.5 Hz - typical braking/accel let mut max_output: f32 = 0.0; for i in 0..200 { - // 10 seconds at 20Hz let t = i as f32 / TELEM_RATE; let input = (2.0 * core::f32::consts::PI * freq * t).sin(); let output = filter.process(input); @@ -337,7 +336,7 @@ mod tests { ); // Test high frequency rejection (8Hz - at Nyquist, should be attenuated) - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF, TELEM_RATE); + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_SLOW, TELEM_RATE); let freq = 8.0; let mut max_output: f32 = 0.0; @@ -358,5 +357,67 @@ mod tests { max_output ); } + + #[test] + fn test_telemetry_rate_filter_actual_config() { + // Test with actual fusion.rs config: 20Hz sampling, 5Hz cutoff + // This is faster/more responsive than conservative 2Hz + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_FAST, TELEM_RATE); + + // DC should pass through + for _ in 0..50 { + filter.process(1.0); + } + let dc_output = filter.process(1.0); + assert!( + (dc_output - 1.0).abs() < 0.02, + "DC should pass at 5Hz cutoff: got {}", + dc_output + ); + + // Test driving frequency (1 Hz - typical braking/accel) + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_FAST, TELEM_RATE); + let freq = 1.0; + let mut max_output: f32 = 0.0; + + for i in 0..200 { + let t = i as f32 / TELEM_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 100 { + max_output = max_output.max(output.abs()); + } + } + + // 1 Hz is well below 5Hz cutoff, should pass through with minimal attenuation + assert!( + max_output > 0.9, + "1Hz should pass at 20Hz/5Hz filter: got {}", + max_output + ); + + // Test vibration rejection (8Hz - should be attenuated even at 5Hz cutoff) + let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_FAST, TELEM_RATE); + let freq = 8.0; + let mut max_output: f32 = 0.0; + + for i in 0..200 { + let t = i as f32 / TELEM_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 100 { + max_output = max_output.max(output.abs()); + } + } + + // 8Hz is above 5Hz cutoff, should be attenuated (but less than at 2Hz cutoff) + assert!( + max_output < 0.5, + "8Hz should be attenuated at 20Hz/5Hz filter: got {}", + max_output + ); + } } diff --git a/sensors/blackbox/src/mode.rs b/sensors/blackbox/src/mode.rs index 7abb0cd..40b7aa6 100644 --- a/sensors/blackbox/src/mode.rs +++ b/sensors/blackbox/src/mode.rs @@ -296,23 +296,24 @@ impl ModeClassifier { /// Update mode using pre-blended hybrid acceleration /// /// This is the preferred method when using SensorFusion, as it receives - /// longitudinal acceleration that's already blended from GPS and IMU sources. + /// longitudinal acceleration that's already blended from GPS and IMU sources, + /// and centripetal lateral acceleration (speed × yaw_rate) for corner detection. /// /// # Arguments /// * `a_lon_blended` - Blended longitudinal acceleration (m/s², vehicle frame) - /// * `a_lat_filtered` - Filtered lateral acceleration (m/s², vehicle frame) + /// * `a_lat_centripetal` - Centripetal lateral acceleration (m/s², = speed × yaw_rate) /// * `wz` - Yaw rate (rad/s) /// * `speed` - Vehicle speed (m/s) - pub fn update_hybrid(&mut self, a_lon_blended: f32, a_lat_filtered: f32, wz: f32, speed: f32) { + pub fn update_hybrid(&mut self, a_lon_blended: f32, a_lat_centripetal: f32, wz: f32, speed: f32) { // Update display speed with EMA self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; if self.v_disp < 3.0 / 3.6 { self.v_disp = 0.0; } - // Convert to g units (blended accel is already in m/s²) + // Convert to g units (accelerations are in m/s²) let a_lon = a_lon_blended / G; - let a_lat = a_lat_filtered / G; + let a_lat = a_lat_centripetal / G; // Update EMA filters // Note: longitudinal already filtered/blended, but still apply EMA for consistency From 3e11999cebe71a9f3450a67492bd53786e55aa0f Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 18:16:21 -0800 Subject: [PATCH 07/30] Reduce mode detection latency by ~50% MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit PROBLEM: Mode detection had ~300ms latency due to double filtering - Biquad (5Hz) in fusion.rs: ~70ms delay - EMA (α=0.35) in mode.rs: ~116ms time constant - Combined: 90% response in ~250-300ms SOLUTION: 1. Remove Biquad filter for longitudinal acceleration - GPS blend already provides smooth signal (no vibration) - 70% GPS / 30% IMU means 70% of signal is already clean - mode.rs EMA handles residual noise 2. Increase EMA α from 0.35 to 0.50 - Time constant reduced from 116ms to 72ms - 90% response: 165ms (was 267ms) RESULT: - Estimated 90% response: ~120-150ms (was ~300ms) - Mode detection should feel significantly more responsive - Corner detection unchanged (uses centripetal, already instant) OTHER CHANGES: - filter.rs moved to #[cfg(test)] - only compiled for tests - Removed unused FusionConfig fields (lon_filter_cutoff, sample_rate) - Updated README to reflect new architecture --- README.md | 4 ++-- sensors/blackbox/src/fusion.rs | 41 ++++++++++++++++------------------ sensors/blackbox/src/main.rs | 11 ++++----- sensors/blackbox/src/mode.rs | 2 +- 4 files changed, 26 insertions(+), 32 deletions(-) diff --git a/README.md b/README.md index 56d1b7e..268f3ff 100644 --- a/README.md +++ b/README.md @@ -26,9 +26,9 @@ A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's - Body-frame accelerations transformed to earth frame using current orientation - Zero-velocity updates eliminate IMU drift when stationary - Constant Turn Rate and Acceleration (CTRA) model for cornering -- **GPS/IMU blending** for accurate mode detection independent of mounting angle +- **GPS/IMU blending** for accurate, low-latency mode detection independent of mounting angle - **Automatic tilt correction** learns device mounting offset when stopped -- **Vibration filtering** removes engine/road noise (5Hz low-pass Butterworth for longitudinal) +- **Centripetal lateral detection** uses speed × yaw_rate for instant corner response **Why it's useful:** - Track day data logging without $1000+ commercial systems diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs index 22d0a7f..53d22bc 100644 --- a/sensors/blackbox/src/fusion.rs +++ b/sensors/blackbox/src/fusion.rs @@ -9,7 +9,9 @@ //! The goal is to provide clean, drift-free acceleration for mode detection //! that works regardless of device mounting angle. -use crate::filter::BiquadFilter; +// Sensor fusion for GPS/IMU blending +// Biquad filter removed from longitudinal path for ~50% faster response. +// GPS blend provides primary smoothing, mode.rs EMA handles residual noise. /// Configuration for sensor fusion #[derive(Clone, Copy)] @@ -38,11 +40,8 @@ pub struct FusionConfig { pub gravity_learn_time: f32, /// Alpha for gravity estimate update (smaller = slower, more stable) pub gravity_alpha: f32, - /// Low-pass filter cutoff for longitudinal acceleration (Hz) - /// Only applied in vehicle frame for longitudinal - pub lon_filter_cutoff: f32, - /// Sample rate for telemetry (Hz) - pub sample_rate: f32, + // Note: lon_filter_cutoff and sample_rate removed - no longer using Biquad filter + // GPS blend provides primary smoothing, mode.rs EMA handles residual noise } impl Default for FusionConfig { @@ -69,8 +68,6 @@ impl Default for FusionConfig { steady_state_yaw_tolerance: 0.087, // ~5 deg/s gravity_learn_time: 2.0, gravity_alpha: 0.02, // Very slow update - lon_filter_cutoff: 5.0, // 5Hz for longitudinal (faster than before) - sample_rate: 20.0, // Telemetry rate } } } @@ -409,10 +406,10 @@ pub struct SensorFusion { pub tilt_estimator: TiltEstimator, /// Gravity estimator (learns while driving) pub gravity_estimator: GravityEstimator, - /// Low-pass filter for longitudinal IMU (applied in vehicle frame) - filter_lon: BiquadFilter, /// Blended longitudinal acceleration (m/s²) blended_lon: f32, + /// Raw IMU longitudinal for display (m/s², vehicle frame) + lon_imu_raw: f32, /// Corrected lateral acceleration for display (m/s², vehicle frame) lat_corrected: f32, /// Centripetal lateral acceleration for mode detection (m/s²) @@ -429,8 +426,8 @@ impl SensorFusion { gps_accel: GpsAcceleration::new(), tilt_estimator: TiltEstimator::new(config.tilt_learn_time), gravity_estimator: GravityEstimator::new(&config), - filter_lon: BiquadFilter::new_lowpass(config.lon_filter_cutoff, config.sample_rate), blended_lon: 0.0, + lon_imu_raw: 0.0, lat_corrected: 0.0, lat_centripetal: 0.0, last_gps_weight: 0.0, @@ -474,13 +471,15 @@ impl SensorFusion { // Apply gravity correction (learned while driving) let (ax_corr, ay_corr) = self.gravity_estimator.correct(ax_tilt, ay_tilt); - // Transform to vehicle frame (NO filtering in earth frame!) + // Transform to vehicle frame let (lon_imu_raw, lat_imu_raw) = earth_to_car(ax_corr, ay_corr, yaw); - // Filter longitudinal in vehicle frame (safe, no rotation issues) - let lon_imu = self.filter_lon.process(lon_imu_raw); + // Store raw IMU longitudinal for display/fallback + // No Biquad filter - GPS blend provides primary smoothing, + // mode.rs EMA handles residual noise. This gives ~50% faster response. + self.lon_imu_raw = lon_imu_raw; - // Store corrected lateral for display (no Biquad filter - mode.rs EMA handles it) + // Store corrected lateral for display self.lat_corrected = lat_imu_raw; // Calculate centripetal lateral for mode detection (pro-style) @@ -492,11 +491,6 @@ impl SensorFusion { // Learn tilt offset when stopped self.tilt_estimator .update_stationary(ax_earth, ay_earth, dt); - - if !self.was_stationary { - // Just stopped - reset filter to avoid transient - self.filter_lon.reset_to(0.0); - } } else { if self.was_stationary { // Just started moving @@ -510,13 +504,16 @@ impl SensorFusion { self.was_stationary = is_stationary; // Blend GPS and IMU for longitudinal acceleration + // GPS provides smooth, drift-free acceleration from velocity changes + // IMU provides fast response but has vibration noise + // Blending gives best of both: smooth when GPS available, fast fallback let gps_weight = self.compute_gps_weight(); self.last_gps_weight = gps_weight; let lon_blended = if let Some(gps_lon) = self.gps_accel.get_accel() { - gps_weight * gps_lon + (1.0 - gps_weight) * lon_imu + gps_weight * gps_lon + (1.0 - gps_weight) * self.lon_imu_raw } else { - lon_imu + self.lon_imu_raw }; self.blended_lon = lon_blended; diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 9e27a97..4b613f9 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -1,7 +1,8 @@ mod binary_telemetry; mod config; mod diagnostics; -mod filter; +#[cfg(test)] +mod filter; // Only compiled for tests - production uses GPS blend instead of Biquad mod fusion; mod gps; mod imu; @@ -453,14 +454,10 @@ fn main() { // 3. Vibration filtering - 2Hz Biquad removes engine/road noise // 4. GPS/IMU blending - 70% GPS at 25Hz for balanced response // - // Latency: ~80-100ms (vs ~50ms IMU-only, ~150ms with 90% GPS) + // Latency: ~100-150ms with GPS blend + mode.rs EMA + // (Biquad filter removed - GPS blend provides primary smoothing) // Good for: city, highway, canyon. For track, increase GPS weights. - // - // NOTE: Filter runs at telemetry rate (~20Hz), not IMU rate (200Hz) - let telemetry_rate = 1000.0 / config.telemetry.interval_ms as f32; let fusion_config = FusionConfig { - sample_rate: telemetry_rate, - lon_filter_cutoff: 5.0, // 5Hz - faster response for longitudinal gps_high_rate: 20.0, gps_medium_rate: 10.0, gps_max_age: 0.2, diff --git a/sensors/blackbox/src/mode.rs b/sensors/blackbox/src/mode.rs index 40b7aa6..bd91060 100644 --- a/sensors/blackbox/src/mode.rs +++ b/sensors/blackbox/src/mode.rs @@ -131,7 +131,7 @@ impl Default for ModeConfig { lat_thr: 0.12, // 0.12g lateral - city turns lat_exit: 0.06, // exit when below 0.06g yaw_thr: 0.05, // ~2.9°/s yaw rate - alpha: 0.35, // EMA smoothing - balanced responsiveness vs bump rejection + alpha: 0.50, // EMA smoothing - faster response (τ≈72ms at 20Hz) } } } From f1131131d165e593c8f7f8f937680807ed34f87f Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 19:20:22 -0800 Subject: [PATCH 08/30] Fix critical bug: settings update was resetting EMA alpha to 0.25 When user changed mode thresholds via web UI, alpha was hardcoded to 0.25 instead of using the optimized 0.50. This would undo the latency improvement after any settings change. Also fixed: - Outdated comment mentioning removed Biquad filter - Misleading comment about lateral (display uses accelerometer, mode uses centripetal) --- sensors/blackbox/src/main.rs | 10 +++++----- sensors/blackbox/src/system.rs | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 4b613f9..90194f7 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -451,11 +451,11 @@ fn main() { // This provides: // 1. Tilt correction - learns mounting offset when stopped (3s) // 2. Gravity correction - learns while driving at steady speed - // 3. Vibration filtering - 2Hz Biquad removes engine/road noise - // 4. GPS/IMU blending - 70% GPS at 25Hz for balanced response + // 3. GPS/IMU blending - 70% GPS at 25Hz for smooth, drift-free longitudinal + // 4. Centripetal lateral - speed × yaw_rate for instant corner detection // - // Latency: ~100-150ms with GPS blend + mode.rs EMA - // (Biquad filter removed - GPS blend provides primary smoothing) + // Latency: ~100-150ms for ACCEL/BRAKE (GPS blend + EMA) + // ~0ms for CORNER (centripetal is instant) // Good for: city, highway, canyon. For track, increase GPS weights. let fusion_config = FusionConfig { gps_high_rate: 20.0, @@ -520,7 +520,7 @@ fn main() { lat_thr: s.lat_thr, lat_exit: s.lat_exit, yaw_thr: s.yaw_thr, - alpha: 0.25, // Keep default smoothing + alpha: 0.50, // Fast EMA for responsive mode detection (τ≈72ms at 20Hz) }; estimator.mode_classifier.update_config(new_config); info!( diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index 08ce688..3cfda31 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -339,9 +339,9 @@ impl TelemetryPublisher { sensor_fusion: &crate::fusion::SensorFusion, now_ms: u32, ) -> Result<(), SystemError> { - // Get accelerations matching what mode classification sees + // Get accelerations for display // lon: GPS/IMU blended (same as mode classifier input) - // lat: filtered IMU (same as mode classifier input) + // lat: accelerometer-based (mode uses centripetal = speed × yaw_rate) let lon_blended = sensor_fusion.get_lon_blended(); let lat_display = sensor_fusion.get_lat_display(); From d2f8a1785332c31e781f1d2606f49733084402bc Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 19:22:08 -0800 Subject: [PATCH 09/30] Add DEFAULT_MODE_ALPHA constant to prevent duplicate values The bug where settings handler used alpha=0.25 while default used 0.50 was caused by the same value being hardcoded in two places. Fix: - Add DEFAULT_MODE_ALPHA constant in mode.rs - Use constant in both ModeConfig::default() and main.rs settings handler - Add unit test to verify default config uses the constant This ensures future changes to alpha only need to update one place. --- sensors/blackbox/src/main.rs | 2 +- sensors/blackbox/src/mode.rs | 18 +++++++++++++++++- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 90194f7..2383b91 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -520,7 +520,7 @@ fn main() { lat_thr: s.lat_thr, lat_exit: s.lat_exit, yaw_thr: s.yaw_thr, - alpha: 0.50, // Fast EMA for responsive mode detection (τ≈72ms at 20Hz) + alpha: mode::DEFAULT_MODE_ALPHA, // Single source of truth }; estimator.mode_classifier.update_config(new_config); info!( diff --git a/sensors/blackbox/src/mode.rs b/sensors/blackbox/src/mode.rs index bd91060..24bbbfa 100644 --- a/sensors/blackbox/src/mode.rs +++ b/sensors/blackbox/src/mode.rs @@ -106,6 +106,11 @@ impl Default for Mode { } } +/// Default EMA alpha for mode detection smoothing +/// τ ≈ 72ms at 20Hz processing rate +/// Used by both ModeConfig::default() and settings handler in main.rs +pub const DEFAULT_MODE_ALPHA: f32 = 0.50; + /// Configurable thresholds for mode detection #[derive(Debug, Clone, Copy)] pub struct ModeConfig { @@ -131,7 +136,7 @@ impl Default for ModeConfig { lat_thr: 0.12, // 0.12g lateral - city turns lat_exit: 0.06, // exit when below 0.06g yaw_thr: 0.05, // ~2.9°/s yaw rate - alpha: 0.50, // EMA smoothing - faster response (τ≈72ms at 20Hz) + alpha: DEFAULT_MODE_ALPHA, } } } @@ -586,4 +591,15 @@ mod tests { assert!(!classifier.get_mode().has_accel(), "Should exit accel"); assert!(classifier.get_mode().has_corner(), "Should still corner"); } + + #[test] + fn test_default_alpha_uses_constant() { + // This test ensures ModeConfig::default() uses the shared constant + // Prevents bugs where alpha is hardcoded in multiple places with different values + let config = ModeConfig::default(); + assert_eq!( + config.alpha, DEFAULT_MODE_ALPHA, + "ModeConfig::default() must use DEFAULT_MODE_ALPHA constant" + ); + } } From 3124bd212ff94efdee1c3b3de8e5d1545c294e3c Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 19:22:38 -0800 Subject: [PATCH 10/30] Add rule about avoiding duplicate constants to CLAUDE.md --- CLAUDE.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CLAUDE.md b/CLAUDE.md index 68289f5..0385081 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -28,6 +28,7 @@ You are a seasoned embedded Rust engineer with deep expertise in: - Avoid over-engineering: solve the problem at hand, not hypothetical future problems - Keep abstractions minimal until they prove necessary - **Never use `#[allow(dead_code)]`** - dead code must be removed or used, not silenced +- **Never duplicate constants** - use a single `const` and reference it everywhere. Add a unit test to verify defaults use the constant. **Testing Philosophy:** - Write meaningful tests that catch real bugs, not boilerplate From 5b6b587516f349dd9532e3389905221347eec9a1 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sat, 17 Jan 2026 19:39:00 -0800 Subject: [PATCH 11/30] Refactor: move pure algorithm modules to framework crate for host testing Moved mode.rs, fusion.rs, and filter.rs from the embedded blackbox crate to the sensor-fusion framework crate. This enables running 52 unit tests on the host machine without requiring ESP32 hardware. Changes: - framework/src/mode.rs: Mode classifier with 10 tests - framework/src/fusion.rs: GPS/IMU blending with 9 tests - framework/src/filter.rs: Biquad filter with 6 tests (unused but verified) - sensors/blackbox/src/*.rs: Thin re-exports from framework - README.md: Updated file structure, added test command Tests now runnable with: cargo test -p sensor-fusion -p wt901 -p ublox-gps --- README.md | 51 ++- framework/src/filter.rs | 230 ++++++++++ framework/src/fusion.rs | 787 ++++++++++++++++++++++++++++++++ framework/src/lib.rs | 9 + framework/src/mode.rs | 611 +++++++++++++++++++++++++ framework/src/transforms.rs | 1 - sensors/blackbox/src/filter.rs | 431 +----------------- sensors/blackbox/src/fusion.rs | 801 +-------------------------------- sensors/blackbox/src/mode.rs | 609 +------------------------ 9 files changed, 1686 insertions(+), 1844 deletions(-) create mode 100644 framework/src/filter.rs create mode 100644 framework/src/fusion.rs create mode 100644 framework/src/mode.rs diff --git a/README.md b/README.md index 268f3ff..94749ff 100644 --- a/README.md +++ b/README.md @@ -310,6 +310,9 @@ cargo fmt # Lint cargo clippy -- -D warnings +# Run unit tests (on host, no device needed) +cargo test -p sensor-fusion -p wt901 -p ublox-gps + # Clean build (if things go wrong) cargo clean ``` @@ -437,28 +440,38 @@ GPS (5-25Hz) IMU (200Hz) ``` blackbox/ +├── framework/ # sensor-fusion library crate (host-testable) +│ └── src/ +│ ├── lib.rs # Crate entry point +│ ├── ekf.rs # Extended Kalman Filter +│ ├── transforms.rs # Body↔Earth↔Vehicle coordinate math +│ ├── mode.rs # Driving mode classifier +│ ├── fusion.rs # GPS/IMU blending, tilt correction +│ ├── filter.rs # Biquad filter (unused, for reference) +│ └── velocity.rs # Velocity source selection +├── drivers/ +│ ├── wt901/ # WT901 IMU driver crate +│ └── ublox-gps/ # u-blox GPS driver crate ├── sensors/ -│ └── blackbox/ +│ └── blackbox/ # ESP32 firmware (embedded binary) │ ├── .cargo/ -│ │ └── config.toml # ESP32-C3 build configuration +│ │ └── config.toml # ESP32-C3 build configuration │ ├── src/ -│ │ ├── main.rs # Main loop and setup -│ │ ├── config.rs # Build-time configuration (GPS model, WiFi mode) -│ │ ├── system.rs # Sensor/estimator/publisher managers -│ │ ├── imu.rs # WT901 UART parser (auto-detect baud) -│ │ ├── gps.rs # NMEA/UBX parser with warmup (NEO-6M/M9N) -│ │ ├── diagnostics.rs # System health monitoring -│ │ ├── transforms.rs # Body↔Earth coordinate math -│ │ ├── mode.rs # Driving mode classifier -│ │ ├── binary_telemetry.rs # 67-byte packet format -│ │ ├── websocket_server.rs # Mobile dashboard & HTTP server -│ │ ├── udp_stream.rs # High-speed UDP client -│ │ ├── mqtt.rs # MQTT client for status -│ │ ├── wifi.rs # WiFi connection manager -│ │ └── rgb_led.rs # WS2812 status LED -│ ├── Cargo.toml # Rust dependencies -│ ├── sdkconfig.defaults # ESP-IDF configuration -│ └── build.rs # Build script +│ │ ├── main.rs # Main loop and setup +│ │ ├── config.rs # Build-time configuration +│ │ ├── system.rs # Sensor/estimator/publisher managers +│ │ ├── imu.rs # WT901 UART parser (auto-detect baud) +│ │ ├── gps.rs # NMEA/UBX parser with warmup +│ │ ├── diagnostics.rs # System health monitoring +│ │ ├── binary_telemetry.rs # 67-byte packet format +│ │ ├── websocket_server.rs # Mobile dashboard & HTTP server +│ │ ├── udp_stream.rs # High-speed UDP client +│ │ ├── mqtt.rs # MQTT client for status +│ │ ├── wifi.rs # WiFi connection manager +│ │ └── rgb_led.rs # WS2812 status LED +│ ├── Cargo.toml # Rust dependencies +│ ├── sdkconfig.defaults # ESP-IDF configuration +│ └── build.rs # Build script ├── tools/ │ └── python/ │ ├── configure_wt901.py # IMU configuration tool (200Hz setup) diff --git a/framework/src/filter.rs b/framework/src/filter.rs new file mode 100644 index 0000000..e46fbd5 --- /dev/null +++ b/framework/src/filter.rs @@ -0,0 +1,230 @@ +//! Signal filtering utilities +//! +//! Contains Biquad IIR filter implementation for vibration removal. +//! Note: Currently unused in production - kept for potential future use +//! and to verify the math with unit tests. + +/// Biquad IIR low-pass filter for vibration removal +/// +/// Implements a 2nd-order Butterworth low-pass filter optimized for +/// removing engine/road vibration (10-100Hz) while preserving +/// driving dynamics (0-3Hz). +/// +/// Design: Butterworth (maximally flat passband, Q = 0.707) +/// Typical cutoff: 4Hz at 200Hz sample rate +#[derive(Clone)] +pub struct BiquadFilter { + // Filter coefficients (normalized) + b0: f32, + b1: f32, + b2: f32, + a1: f32, + a2: f32, + + // Filter state (previous samples) + x1: f32, // x[n-1] + x2: f32, // x[n-2] + y1: f32, // y[n-1] + y2: f32, // y[n-2] +} + +impl BiquadFilter { + /// Create a new Butterworth low-pass filter + /// + /// # Arguments + /// * `cutoff_hz` - Cutoff frequency in Hz (e.g., 4.0) + /// * `sample_rate_hz` - Sample rate in Hz (e.g., 200.0) + /// + /// # Panics + /// Panics if cutoff >= sample_rate/2 (Nyquist limit) + pub fn new_lowpass(cutoff_hz: f32, sample_rate_hz: f32) -> Self { + assert!( + cutoff_hz < sample_rate_hz / 2.0, + "Cutoff must be below Nyquist frequency" + ); + + // Butterworth Q factor for critically damped response + let q = core::f32::consts::FRAC_1_SQRT_2; // 0.7071... + + // Pre-warp the cutoff frequency for bilinear transform + let omega = 2.0 * core::f32::consts::PI * cutoff_hz / sample_rate_hz; + let sin_omega = omega.sin(); + let cos_omega = omega.cos(); + let alpha = sin_omega / (2.0 * q); + + // Compute coefficients (lowpass) + let b0 = (1.0 - cos_omega) / 2.0; + let b1 = 1.0 - cos_omega; + let b2 = (1.0 - cos_omega) / 2.0; + let a0 = 1.0 + alpha; + let a1 = -2.0 * cos_omega; + let a2 = 1.0 - alpha; + + // Normalize by a0 + Self { + b0: b0 / a0, + b1: b1 / a0, + b2: b2 / a0, + a1: a1 / a0, + a2: a2 / a0, + x1: 0.0, + x2: 0.0, + y1: 0.0, + y2: 0.0, + } + } + + /// Process a single sample through the filter + /// + /// # Arguments + /// * `input` - Input sample + /// + /// # Returns + /// Filtered output sample + pub fn process(&mut self, input: f32) -> f32 { + // Direct Form I implementation + // y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] + let output = self.b0 * input + self.b1 * self.x1 + self.b2 * self.x2 + - self.a1 * self.y1 + - self.a2 * self.y2; + + // Update state + self.x2 = self.x1; + self.x1 = input; + self.y2 = self.y1; + self.y1 = output; + + output + } + + /// Initialize filter state to a steady value + /// + /// Use this to avoid startup transients when you know + /// the initial value (e.g., after calibration shows ~0) + pub fn reset_to(&mut self, value: f32) { + self.x1 = value; + self.x2 = value; + self.y1 = value; + self.y2 = value; + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const SAMPLE_RATE: f32 = 200.0; + const CUTOFF: f32 = 4.0; + + #[test] + fn test_filter_creation() { + let filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + assert!(filter.b0 > 0.0 && filter.b0 < 1.0); + assert!(filter.b1 > 0.0); + assert!(filter.b2 > 0.0 && filter.b2 < 1.0); + } + + #[test] + #[should_panic(expected = "Cutoff must be below Nyquist")] + fn test_filter_nyquist_limit() { + let _ = BiquadFilter::new_lowpass(100.0, SAMPLE_RATE); + } + + #[test] + fn test_dc_passthrough() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + for _ in 0..100 { + filter.process(1.0); + } + + let output = filter.process(1.0); + assert!( + (output - 1.0).abs() < 0.01, + "DC should pass through: got {}", + output + ); + } + + #[test] + fn test_high_frequency_attenuation() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let freq = 50.0; + let mut max_output: f32 = 0.0; + + for i in 0..1000 { + let t = i as f32 / SAMPLE_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 500 { + max_output = max_output.max(output.abs()); + } + } + + assert!( + max_output < 0.05, + "50Hz should be attenuated: got peak {}", + max_output + ); + } + + #[test] + fn test_low_frequency_passthrough() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let freq = 1.0; + let mut max_output: f32 = 0.0; + + for i in 0..2000 { + let t = i as f32 / SAMPLE_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 1000 { + max_output = max_output.max(output.abs()); + } + } + + assert!( + max_output > 0.9, + "1Hz should pass through: got peak {}", + max_output + ); + } + + #[test] + fn test_step_response_minimal_overshoot() { + // Butterworth filters have minimal overshoot (Q = 0.707) + // In practice, slight overshoot ~5% is acceptable + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let mut max_output: f32 = 0.0; + + for _ in 0..500 { + let output = filter.process(1.0); + max_output = max_output.max(output); + } + + assert!( + max_output <= 1.10, + "Should have minimal overshoot (<10%): got max {}", + max_output + ); + } + + #[test] + fn test_reset_to_value() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + filter.reset_to(0.5); + + let output = filter.process(0.5); + assert!( + (output - 0.5).abs() < 0.01, + "After reset_to, should output steady value: got {}", + output + ); + } +} diff --git a/framework/src/fusion.rs b/framework/src/fusion.rs new file mode 100644 index 0000000..323795f --- /dev/null +++ b/framework/src/fusion.rs @@ -0,0 +1,787 @@ +//! Sensor Fusion Module +//! +//! This module handles: +//! 1. GPS-derived longitudinal acceleration (from velocity changes) +//! 2. GPS/IMU acceleration blending with adaptive weights +//! 3. Dynamic tilt offset learning (ZUPT enhancement) +//! 4. Moving gravity estimation (continuous calibration while driving) +//! +//! The goal is to provide clean, drift-free acceleration for mode detection +//! that works regardless of device mounting angle. + +// Sensor fusion for GPS/IMU blending +// Biquad filter removed from longitudinal path for ~50% faster response. +// GPS blend provides primary smoothing, mode.rs EMA handles residual noise. + +use crate::transforms::earth_to_car; + +/// Configuration for sensor fusion +#[derive(Clone, Copy)] +pub struct FusionConfig { + /// GPS rate threshold for high-confidence GPS (Hz) + pub gps_high_rate: f32, + /// GPS rate threshold for medium-confidence GPS (Hz) + pub gps_medium_rate: f32, + /// Maximum age of GPS data before considered stale (seconds) + pub gps_max_age: f32, + /// GPS weight when rate >= high_rate (0.0-1.0) + /// Higher = more GPS, smoother but slower response + /// Lower = more IMU, faster response but potential drift + pub gps_weight_high: f32, + /// GPS weight when rate >= medium_rate (0.0-1.0) + pub gps_weight_medium: f32, + /// GPS weight when rate < medium_rate (0.0-1.0) + pub gps_weight_low: f32, + /// Time required at stop to learn tilt offset (seconds) + pub tilt_learn_time: f32, + /// Speed threshold for steady-state detection (m/s) + pub steady_state_speed_tolerance: f32, + /// Yaw rate threshold for steady-state (rad/s) + pub steady_state_yaw_tolerance: f32, + /// Time required in steady state to update gravity estimate (seconds) + pub gravity_learn_time: f32, + /// Alpha for gravity estimate update (smaller = slower, more stable) + pub gravity_alpha: f32, + // Note: lon_filter_cutoff and sample_rate removed - no longer using Biquad filter + // GPS blend provides primary smoothing, mode.rs EMA handles residual noise +} + +impl Default for FusionConfig { + /// Default configuration balanced for city/highway/canyon driving. + /// + /// GPS weights are moderate (70/50/30) for balanced responsiveness: + /// - Fast enough for canyon driving (~80-100ms latency) + /// - Smooth enough for city/highway (no jitter) + /// + /// For track use, consider increasing GPS weights (90/70/50) for + /// maximum accuracy at the cost of some latency. + fn default() -> Self { + Self { + gps_high_rate: 20.0, + gps_medium_rate: 10.0, + gps_max_age: 0.2, // 200ms + // Balanced blend: 70% GPS when fresh and fast + // Gives ~80-100ms latency instead of ~120-150ms + gps_weight_high: 0.70, // 70% GPS / 30% IMU at >= 20Hz + gps_weight_medium: 0.50, // 50% GPS / 50% IMU at >= 10Hz + gps_weight_low: 0.30, // 30% GPS / 70% IMU at < 10Hz + tilt_learn_time: 3.0, + steady_state_speed_tolerance: 0.5, // 0.5 m/s = 1.8 km/h + steady_state_yaw_tolerance: 0.087, // ~5 deg/s + gravity_learn_time: 2.0, + gravity_alpha: 0.02, // Very slow update + } + } +} + +/// GPS-derived acceleration tracker +pub struct GpsAcceleration { + /// Previous speed (m/s) + prev_speed: f32, + /// Previous timestamp (seconds, monotonic) + prev_time: f32, + /// Computed longitudinal acceleration (m/s²) + accel_lon: f32, + /// Is the acceleration estimate valid? + valid: bool, + /// Current GPS rate estimate (Hz) + gps_rate: f32, + /// Time since last valid GPS fix (seconds) + time_since_fix: f32, + /// EMA for GPS rate calculation + rate_ema: f32, + /// Last fix count for rate calculation + last_fix_count: u32, + /// Time of last rate calculation + last_rate_time: f32, +} + +impl GpsAcceleration { + pub fn new() -> Self { + Self { + prev_speed: 0.0, + prev_time: 0.0, + accel_lon: 0.0, + valid: false, + gps_rate: 0.0, + time_since_fix: 999.0, + rate_ema: 0.0, + last_fix_count: 0, + last_rate_time: 0.0, + } + } + + /// Update with new GPS speed measurement + /// + /// # Arguments + /// * `speed` - Current speed in m/s + /// * `time` - Current timestamp in seconds (monotonic) + /// + /// # Returns + /// The computed longitudinal acceleration, or None if not enough data + pub fn update(&mut self, speed: f32, time: f32) -> Option { + self.time_since_fix = 0.0; + + if self.prev_time > 0.0 { + let dt = time - self.prev_time; + + // Validate dt: should be 20-200ms for 5-50Hz GPS + if dt > 0.02 && dt < 0.5 { + self.accel_lon = (speed - self.prev_speed) / dt; + self.valid = true; + + self.prev_speed = speed; + self.prev_time = time; + + return Some(self.accel_lon); + } + } + + self.prev_speed = speed; + self.prev_time = time; + None + } + + /// Update GPS rate estimate + /// + /// # Arguments + /// * `fix_count` - Total number of valid fixes received + /// * `time` - Current timestamp in seconds + pub fn update_rate(&mut self, fix_count: u32, time: f32) { + if self.last_rate_time > 0.0 { + let dt = time - self.last_rate_time; + if dt >= 1.0 { + let fixes = fix_count.saturating_sub(self.last_fix_count); + let instant_rate = fixes as f32 / dt; + + // EMA smoothing for rate + self.rate_ema = 0.3 * instant_rate + 0.7 * self.rate_ema; + self.gps_rate = self.rate_ema; + + self.last_fix_count = fix_count; + self.last_rate_time = time; + } + } else { + self.last_fix_count = fix_count; + self.last_rate_time = time; + } + } + + /// Advance time without GPS fix (for staleness tracking) + pub fn advance_time(&mut self, dt: f32) { + self.time_since_fix += dt; + if self.time_since_fix > 0.5 { + self.valid = false; + } + } + + /// Get current GPS-derived acceleration + pub fn get_accel(&self) -> Option { + if self.valid { + Some(self.accel_lon) + } else { + None + } + } + + /// Get current GPS rate estimate (Hz) + pub fn get_rate(&self) -> f32 { + self.gps_rate + } + + /// Check if GPS data is fresh + pub fn is_fresh(&self, max_age: f32) -> bool { + self.time_since_fix < max_age + } +} + +impl Default for GpsAcceleration { + fn default() -> Self { + Self::new() + } +} + +/// Tilt offset estimator - learns mounting offset when stopped +pub struct TiltEstimator { + /// Accumulated X acceleration (earth frame) + accel_x_sum: f32, + /// Accumulated Y acceleration (earth frame) + accel_y_sum: f32, + /// Number of samples accumulated + sample_count: u32, + /// Time stationary (seconds) + stationary_time: f32, + /// Learned tilt offset X (m/s²) + offset_x: f32, + /// Learned tilt offset Y (m/s²) + offset_y: f32, + /// Is the offset valid (learned at least once)? + offset_valid: bool, + /// Required stationary time to learn (seconds) + learn_time: f32, +} + +impl TiltEstimator { + pub fn new(learn_time: f32) -> Self { + Self { + accel_x_sum: 0.0, + accel_y_sum: 0.0, + sample_count: 0, + stationary_time: 0.0, + offset_x: 0.0, + offset_y: 0.0, + offset_valid: false, + learn_time, + } + } + + /// Update with acceleration sample while stationary + /// + /// # Arguments + /// * `ax_earth` - X acceleration in earth frame (m/s²) + /// * `ay_earth` - Y acceleration in earth frame (m/s²) + /// * `dt` - Time step (seconds) + /// + /// # Returns + /// true if offset was just updated (can trigger visual feedback) + pub fn update_stationary(&mut self, ax_earth: f32, ay_earth: f32, dt: f32) -> bool { + self.stationary_time += dt; + self.accel_x_sum += ax_earth; + self.accel_y_sum += ay_earth; + self.sample_count += 1; + + // After sufficient time, compute offset + if self.stationary_time >= self.learn_time && self.sample_count > 50 { + let new_offset_x = self.accel_x_sum / self.sample_count as f32; + let new_offset_y = self.accel_y_sum / self.sample_count as f32; + + // Only update if change is significant (avoid jitter) + let change = ((new_offset_x - self.offset_x).powi(2) + + (new_offset_y - self.offset_y).powi(2)) + .sqrt(); + + if change > 0.05 || !self.offset_valid { + self.offset_x = new_offset_x; + self.offset_y = new_offset_y; + self.offset_valid = true; + + // Reset accumulators for continued learning + self.accel_x_sum = 0.0; + self.accel_y_sum = 0.0; + self.sample_count = 0; + + return true; + } + } + + false + } + + /// Reset stationary tracking (called when vehicle starts moving) + pub fn reset_stationary(&mut self) { + self.accel_x_sum = 0.0; + self.accel_y_sum = 0.0; + self.sample_count = 0; + self.stationary_time = 0.0; + } + + /// Apply tilt correction to acceleration + pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { + if self.offset_valid { + (ax_earth - self.offset_x, ay_earth - self.offset_y) + } else { + (ax_earth, ay_earth) + } + } +} + +/// Moving gravity estimator - learns gravity offset while driving +pub struct GravityEstimator { + /// Estimated gravity offset X (earth frame, m/s²) + offset_x: f32, + /// Estimated gravity offset Y (earth frame, m/s²) + offset_y: f32, + /// Is the estimate valid? + valid: bool, + /// Speed history for steady-state detection + speed_history: [f32; 10], + speed_idx: usize, + /// Time in steady state (seconds) + steady_time: f32, + /// Configuration + speed_tolerance: f32, + yaw_tolerance: f32, + learn_time: f32, + alpha: f32, +} + +impl GravityEstimator { + pub fn new(config: &FusionConfig) -> Self { + Self { + offset_x: 0.0, + offset_y: 0.0, + valid: false, + speed_history: [0.0; 10], + speed_idx: 0, + steady_time: 0.0, + speed_tolerance: config.steady_state_speed_tolerance, + yaw_tolerance: config.steady_state_yaw_tolerance, + learn_time: config.gravity_learn_time, + alpha: config.gravity_alpha, + } + } + + /// Update with current driving state + /// + /// # Arguments + /// * `ax_earth` - X acceleration (earth frame, m/s²) + /// * `ay_earth` - Y acceleration (earth frame, m/s²) + /// * `speed` - Current speed (m/s) + /// * `yaw_rate` - Current yaw rate (rad/s) + /// * `dt` - Time step (seconds) + pub fn update( + &mut self, + ax_earth: f32, + ay_earth: f32, + speed: f32, + yaw_rate: f32, + dt: f32, + ) -> bool { + // Update speed history + self.speed_history[self.speed_idx] = speed; + self.speed_idx = (self.speed_idx + 1) % self.speed_history.len(); + + // Check if in steady state (constant velocity) + let speed_stable = self.is_speed_stable(); + let yaw_low = yaw_rate.abs() < self.yaw_tolerance; + let moving = speed > 2.0; // Must be moving (not stopped) + + if speed_stable && yaw_low && moving { + self.steady_time += dt; + + if self.steady_time >= self.learn_time { + // In steady state, expected acceleration is ~0 + // Any measured acceleration is gravity/mounting error + self.offset_x = (1.0 - self.alpha) * self.offset_x + self.alpha * ax_earth; + self.offset_y = (1.0 - self.alpha) * self.offset_y + self.alpha * ay_earth; + self.valid = true; + return true; + } + } else { + self.steady_time = 0.0; + } + + false + } + + fn is_speed_stable(&self) -> bool { + if self.speed_history[0] < 1.0 { + return false; // Need some speed data + } + + let mean: f32 = self.speed_history.iter().sum::() / self.speed_history.len() as f32; + let max_dev = self + .speed_history + .iter() + .map(|&s| (s - mean).abs()) + .fold(0.0f32, |a, b| a.max(b)); + + max_dev < self.speed_tolerance + } + + /// Apply gravity correction to acceleration + pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { + if self.valid { + (ax_earth - self.offset_x, ay_earth - self.offset_y) + } else { + (ax_earth, ay_earth) + } + } +} + +/// Main sensor fusion processor +pub struct SensorFusion { + pub config: FusionConfig, + /// GPS-derived acceleration + pub gps_accel: GpsAcceleration, + /// Tilt estimator (learns when stopped) + pub tilt_estimator: TiltEstimator, + /// Gravity estimator (learns while driving) + pub gravity_estimator: GravityEstimator, + /// Blended longitudinal acceleration (m/s²) + blended_lon: f32, + /// Raw IMU longitudinal for display (m/s², vehicle frame) + lon_imu_raw: f32, + /// Corrected lateral acceleration for display (m/s², vehicle frame) + lat_corrected: f32, + /// Centripetal lateral acceleration for mode detection (m/s²) + lat_centripetal: f32, + /// Last GPS blend weight (for diagnostics) + last_gps_weight: f32, + /// Was stationary last update? + was_stationary: bool, +} + +impl SensorFusion { + pub fn new(config: FusionConfig) -> Self { + Self { + gps_accel: GpsAcceleration::new(), + tilt_estimator: TiltEstimator::new(config.tilt_learn_time), + gravity_estimator: GravityEstimator::new(&config), + blended_lon: 0.0, + lon_imu_raw: 0.0, + lat_corrected: 0.0, + lat_centripetal: 0.0, + last_gps_weight: 0.0, + was_stationary: false, + config, + } + } + + /// Process IMU data (call at telemetry rate, e.g., 20Hz) + /// + /// # Arguments + /// * `ax_earth` - X acceleration in earth frame (m/s²) + /// * `ay_earth` - Y acceleration in earth frame (m/s²) + /// * `yaw` - Vehicle yaw angle (rad, for earth-to-car transform) + /// * `speed` - Vehicle speed (m/s) + /// * `yaw_rate` - Yaw rate (rad/s) + /// * `dt` - Time step (seconds) + /// * `is_stationary` - Whether vehicle is currently stationary + /// + /// # Returns + /// (lon_blended, lat_centripetal) - Accelerations for mode detection (m/s²) + /// lon_blended: GPS/IMU blended longitudinal + /// lat_centripetal: speed * yaw_rate (pro-style, mount-independent) + #[allow(clippy::too_many_arguments)] + pub fn process_imu( + &mut self, + ax_earth: f32, + ay_earth: f32, + yaw: f32, + speed: f32, + yaw_rate: f32, + dt: f32, + is_stationary: bool, + ) -> (f32, f32) { + // Track GPS staleness + self.gps_accel.advance_time(dt); + + // Apply tilt correction (learned when stopped) + let (ax_tilt, ay_tilt) = self.tilt_estimator.correct(ax_earth, ay_earth); + + // Apply gravity correction (learned while driving) + let (ax_corr, ay_corr) = self.gravity_estimator.correct(ax_tilt, ay_tilt); + + // Transform to vehicle frame + let (lon_imu_raw, lat_imu_raw) = earth_to_car(ax_corr, ay_corr, yaw); + + // Store raw IMU longitudinal for display/fallback + // No Biquad filter - GPS blend provides primary smoothing, + // mode.rs EMA handles residual noise. This gives ~50% faster response. + self.lon_imu_raw = lon_imu_raw; + + // Store corrected lateral for display + self.lat_corrected = lat_imu_raw; + + // Calculate centripetal lateral for mode detection (pro-style) + // a_lateral = v * omega (mount-angle independent, no filtering needed) + self.lat_centripetal = speed * yaw_rate; + + // Handle stationary state transitions + if is_stationary { + // Learn tilt offset when stopped + self.tilt_estimator + .update_stationary(ax_earth, ay_earth, dt); + } else { + if self.was_stationary { + // Just started moving + self.tilt_estimator.reset_stationary(); + } + + // Update gravity estimate while driving + self.gravity_estimator + .update(ax_earth, ay_earth, speed, yaw_rate, dt); + } + self.was_stationary = is_stationary; + + // Blend GPS and IMU for longitudinal acceleration + // GPS provides smooth, drift-free acceleration from velocity changes + // IMU provides fast response but has vibration noise + // Blending gives best of both: smooth when GPS available, fast fallback + let gps_weight = self.compute_gps_weight(); + self.last_gps_weight = gps_weight; + + let lon_blended = if let Some(gps_lon) = self.gps_accel.get_accel() { + gps_weight * gps_lon + (1.0 - gps_weight) * self.lon_imu_raw + } else { + self.lon_imu_raw + }; + + self.blended_lon = lon_blended; + + // Return blended longitudinal and centripetal lateral for mode detection + (lon_blended, self.lat_centripetal) + } + + /// Process GPS speed update (call at GPS rate, e.g., 25Hz) + /// + /// # Arguments + /// * `speed` - GPS speed in m/s + /// * `time` - Monotonic timestamp in seconds + pub fn process_gps(&mut self, speed: f32, time: f32) { + self.gps_accel.update(speed, time); + } + + /// Update GPS rate estimate (call periodically, e.g., every second) + pub fn update_gps_rate(&mut self, fix_count: u32, time: f32) { + self.gps_accel.update_rate(fix_count, time); + } + + /// Get blended longitudinal acceleration (vehicle frame, m/s²) + /// This is GPS/IMU blended - same value used by mode classification + /// Positive = forward acceleration + pub fn get_lon_blended(&self) -> f32 { + self.blended_lon + } + + /// Get corrected lateral acceleration for display (vehicle frame, m/s²) + /// This is accelerometer-based with tilt/gravity correction + /// Positive = left + pub fn get_lat_display(&self) -> f32 { + self.lat_corrected + } + + /// Compute GPS weight based on GPS rate and data freshness + fn compute_gps_weight(&self) -> f32 { + let rate = self.gps_accel.get_rate(); + let fresh = self.gps_accel.is_fresh(self.config.gps_max_age); + + if !fresh { + return 0.0; + } + + // Blending based on GPS rate using configurable weights + if rate >= self.config.gps_high_rate { + self.config.gps_weight_high + } else if rate >= self.config.gps_medium_rate { + self.config.gps_weight_medium + } else if rate > 0.0 { + self.config.gps_weight_low + } else { + 0.0 + } + } +} + +impl Default for SensorFusion { + fn default() -> Self { + Self::new(FusionConfig::default()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_gps_acceleration_basic() { + let mut gps = GpsAcceleration::new(); + + // First update - no acceleration yet (use non-zero time) + assert!(gps.update(10.0, 1.0).is_none()); + + // Second update - should compute acceleration + let accel = gps.update(12.0, 1.1); // 2 m/s increase over 0.1s = 20 m/s² + assert!(accel.is_some()); + assert!((accel.unwrap() - 20.0).abs() < 0.1); + } + + #[test] + fn test_gps_acceleration_decel() { + let mut gps = GpsAcceleration::new(); + + // Use non-zero times (prev_time > 0.0 check in code) + gps.update(20.0, 1.0); + let accel = gps.update(15.0, 1.1); // 5 m/s decrease = -50 m/s² + + assert!(accel.is_some()); + assert!((accel.unwrap() + 50.0).abs() < 0.1); + } + + #[test] + fn test_gps_staleness() { + let mut gps = GpsAcceleration::new(); + let config = FusionConfig::default(); + + // Use non-zero times + gps.update(10.0, 1.0); + gps.update(12.0, 1.1); + + assert!(gps.is_fresh(config.gps_max_age)); + assert!(gps.get_accel().is_some()); + + // Advance time without GPS update - exceeds freshness threshold + gps.advance_time(0.3); // > 200ms (gps_max_age) + assert!(!gps.is_fresh(config.gps_max_age)); + + // Advance more time - exceeds validity threshold (0.5s internal) + gps.advance_time(0.3); // total 0.6s > 0.5s + assert!(gps.get_accel().is_none()); + } + + #[test] + fn test_tilt_estimator_learns_and_corrects() { + let mut tilt = TiltEstimator::new(1.0); // 1 second learn time + + // Simulate stationary with 0.5 m/s² offset (tilted mount) + for _ in 0..300 { + // 1.5 seconds at 200Hz + tilt.update_stationary(0.5, 0.3, 0.005); + } + + // Correction should now reduce the offset to near zero + let (ax, ay) = tilt.correct(0.5, 0.3); + assert!(ax.abs() < 0.1, "Corrected X should be ~0: {}", ax); + assert!(ay.abs() < 0.1, "Corrected Y should be ~0: {}", ay); + } + + #[test] + fn test_centripetal_lateral_during_turn() { + // Simulate a turn: speed = 10 m/s, yaw_rate = 0.3 rad/s + // Expected centripetal: 10 * 0.3 = 3 m/s² + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Process with turn conditions + let (_lon, lat) = fusion.process_imu( + 0.0, // No earth-frame X accel + 0.0, // No earth-frame Y accel + 0.0, // Heading + 10.0, // Speed = 10 m/s + 0.3, // Yaw rate = 0.3 rad/s (turning) + 0.05, // dt + false, // Not stationary + ); + + // Lateral should be centripetal: speed * yaw_rate = 3 m/s² + assert!( + (lat - 3.0).abs() < 0.01, + "Centripetal lateral should be 3.0: got {}", + lat + ); + } + + #[test] + fn test_lateral_returns_to_zero_after_turn() { + // This test verifies the critical bug fix: + // After a turn ends, lateral should return to zero immediately + // (not "stick" due to earth-frame filtering) + + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Simulate a 90° turn + let mut yaw = 0.0; + for i in 0..20 { + // 1 second of turning at 20Hz + let yaw_rate = 1.57; // ~90°/s + yaw += yaw_rate * 0.05; + + let (_lon, lat) = fusion.process_imu( + 0.0, // Earth X + 0.0, // Earth Y + yaw, // Changing heading + 10.0, // Constant speed + yaw_rate, // Turning + 0.05, // dt + false, // Not stationary + ); + + // During turn, lateral should be positive (speed * positive yaw_rate) + if i > 5 { + assert!( + lat > 10.0, + "During turn, lateral should be high: {}", + lat + ); + } + } + + // Turn ends - yaw_rate goes to zero + let (_lon, lat) = fusion.process_imu( + 0.0, // Earth X + 0.0, // Earth Y + yaw, // Final heading + 10.0, // Still moving + 0.0, // NO MORE TURNING + 0.05, // dt + false, + ); + + // Lateral should be zero immediately (no sticking!) + assert!( + lat.abs() < 0.01, + "After turn ends, lateral should be ~0 immediately: got {}", + lat + ); + } + + #[test] + fn test_display_matches_mode_classification_longitudinal() { + // Verify that get_lon_blended() returns the same value as process_imu() + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Setup GPS + fusion.gps_accel.rate_ema = 25.0; + fusion.gps_accel.update(10.0, 0.0); + fusion.gps_accel.update(12.0, 0.04); + + let (lon_from_process, _lat) = fusion.process_imu(5.0, 3.0, 0.0, 10.0, 0.0, 0.05, false); + + let lon_for_display = fusion.get_lon_blended(); + + assert_eq!( + lon_from_process, lon_for_display, + "Display lon must match mode classifier input" + ); + } + + #[test] + fn test_earth_to_car_transform() { + // Heading East (yaw = 0) + let (lon, lat) = earth_to_car(10.0, 0.0, 0.0); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + + // Heading North (yaw = π/2) + let (lon, lat) = earth_to_car(0.0, 10.0, core::f32::consts::FRAC_PI_2); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + + // Heading West (yaw = π) + let (lon, lat) = earth_to_car(-10.0, 0.0, core::f32::consts::PI); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + } + + #[test] + fn test_gps_rate_calculation() { + let mut gps = GpsAcceleration::new(); + + // Initialize with non-zero time + gps.update_rate(0, 1.0); + + // After 1 second with 25 fixes + gps.update_rate(25, 2.0); + // First update, EMA: 0.3 * 25 + 0.7 * 0 = 7.5 + assert!((gps.get_rate() - 7.5).abs() < 1.0, "First rate should be ~7.5: got {}", gps.get_rate()); + + // After another second with 25 more fixes + gps.update_rate(50, 3.0); + let rate = gps.get_rate(); + // EMA: 0.3 * 25 + 0.7 * 7.5 = 7.5 + 5.25 = 12.75 + assert!(rate > 10.0 && rate < 20.0, "Rate should converge: {}", rate); + } +} diff --git a/framework/src/lib.rs b/framework/src/lib.rs index 38f20d8..8cfbc11 100644 --- a/framework/src/lib.rs +++ b/framework/src/lib.rs @@ -132,11 +132,20 @@ // Core framework modules pub mod ekf; +pub mod filter; +pub mod fusion; +pub mod mode; pub mod sensor_framework; pub mod sensors; pub mod transforms; pub mod velocity; +// Re-export mode detection types +pub use mode::{Mode, ModeClassifier, ModeConfig, DEFAULT_MODE_ALPHA}; + +// Re-export sensor fusion types +pub use fusion::{FusionConfig, GpsAcceleration, GravityEstimator, SensorFusion, TiltEstimator}; + // Re-export commonly used types from sensor_framework // Re-export EKF pub use ekf::Ekf; diff --git a/framework/src/mode.rs b/framework/src/mode.rs new file mode 100644 index 0000000..cf2d16b --- /dev/null +++ b/framework/src/mode.rs @@ -0,0 +1,611 @@ +#![allow(dead_code)] // API methods for future use + +/// Driving mode classifier +/// Detects IDLE, ACCEL, BRAKE, CORNER, ACCEL+CORNER, and BRAKE+CORNER modes +/// based on acceleration and yaw rate +/// +/// ## Hybrid Acceleration Support +/// +/// This classifier can operate in two modes: +/// 1. **Legacy mode**: Uses `update()` with raw earth-frame IMU accelerations +/// 2. **Hybrid mode**: Uses `update_hybrid()` with pre-blended longitudinal acceleration +/// +/// Hybrid mode receives longitudinal acceleration that's already blended from +/// GPS-derived (from velocity changes) and IMU sources. This provides: +/// - Drift-free longitudinal detection (GPS doesn't drift) +/// - Mounting angle independence (GPS measures actual vehicle acceleration) +/// - Graceful fallback when GPS is unavailable +use crate::transforms::earth_to_car; + +const G: f32 = 9.80665; // m/s² + +/// Driving mode flags - can be combined +pub struct Mode { + flags: u8, +} + +// Mode bit flags +const IDLE: u8 = 0; +const ACCEL: u8 = 1; +const BRAKE: u8 = 2; +const CORNER: u8 = 4; + +impl Mode { + pub fn idle() -> Self { + Self { flags: IDLE } + } + + pub fn is_idle(&self) -> bool { + self.flags == IDLE + } + + pub fn has_accel(&self) -> bool { + self.flags & ACCEL != 0 + } + + pub fn has_brake(&self) -> bool { + self.flags & BRAKE != 0 + } + + pub fn has_corner(&self) -> bool { + self.flags & CORNER != 0 + } + + pub fn set_accel(&mut self, enabled: bool) { + if enabled { + self.flags |= ACCEL; + self.flags &= !BRAKE; // Clear brake (mutually exclusive) + } else { + self.flags &= !ACCEL; + } + } + + pub fn set_brake(&mut self, enabled: bool) { + if enabled { + self.flags |= BRAKE; + self.flags &= !ACCEL; // Clear accel (mutually exclusive) + } else { + self.flags &= !BRAKE; + } + } + + pub fn set_corner(&mut self, enabled: bool) { + if enabled { + self.flags |= CORNER; + } else { + self.flags &= !CORNER; + } + } + + pub fn clear(&mut self) { + self.flags = IDLE; + } + + /// Get mode as string for display + pub fn as_str(&self) -> &'static str { + match self.flags { + 0 => "IDLE", + 1 => "ACCEL", + 2 => "BRAKE", + 4 => "CORNER", + 5 => "ACCEL+CORNER", + 6 => "BRAKE+CORNER", + _ => "UNKNOWN", + } + } + + /// Get mode as u8 for binary telemetry + pub fn as_u8(&self) -> u8 { + self.flags + } +} + +impl Default for Mode { + fn default() -> Self { + Self::idle() + } +} + +/// Default EMA alpha for mode detection smoothing +/// τ ≈ 72ms at 20Hz processing rate +/// Used by both ModeConfig::default() and settings handler in main.rs +pub const DEFAULT_MODE_ALPHA: f32 = 0.50; + +/// Configurable thresholds for mode detection +#[derive(Debug, Clone, Copy)] +pub struct ModeConfig { + pub min_speed: f32, // m/s (minimum speed for maneuvers) + pub acc_thr: f32, // g (acceleration threshold) + pub acc_exit: f32, // g (acceleration exit threshold) + pub brake_thr: f32, // g (braking threshold, negative) + pub brake_exit: f32, // g (braking exit threshold, negative) + pub lat_thr: f32, // g (lateral acceleration threshold) + pub lat_exit: f32, // g (lateral acceleration exit threshold) + pub yaw_thr: f32, // rad/s (yaw rate threshold for cornering) + pub alpha: f32, // EMA smoothing factor +} + +impl Default for ModeConfig { + fn default() -> Self { + Self { + min_speed: 2.0, // ~7 km/h - must be moving for accel/brake/corner + acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) + acc_exit: 0.05, // exit when below 0.05g + brake_thr: -0.18, // -0.18g - city driving (normal braking) + brake_exit: -0.09, // exit when above -0.09g + lat_thr: 0.12, // 0.12g lateral - city turns + lat_exit: 0.06, // exit when below 0.06g + yaw_thr: 0.05, // ~2.9°/s yaw rate + alpha: DEFAULT_MODE_ALPHA, + } + } +} + +/// Mode classifier with EMA filtering +pub struct ModeClassifier { + pub mode: Mode, + pub config: ModeConfig, + + // EMA filtered values (all accelerations filtered to reject road bumps) + a_lon_ema: f32, + a_lat_ema: f32, + yaw_ema: f32, + + // Display speed with separate smoothing + v_disp: f32, + v_alpha: f32, // Speed display smoothing factor +} + +impl ModeClassifier { + pub fn new() -> Self { + Self { + mode: Mode::default(), + config: ModeConfig::default(), + a_lon_ema: 0.0, + a_lat_ema: 0.0, + yaw_ema: 0.0, + v_disp: 0.0, + v_alpha: 0.85, // High alpha = fast response to GPS speed changes + } + } + + /// Update mode based on current vehicle state + /// + /// # Arguments + /// * `ax_earth` - Longitudinal acceleration in earth frame (m/s²) + /// * `ay_earth` - Lateral acceleration in earth frame (m/s²) + /// * `yaw_rad` - Vehicle yaw angle (rad) + /// * `wz` - Yaw rate (rad/s) + /// * `speed` - Vehicle speed (m/s) + pub fn update(&mut self, ax_earth: f32, ay_earth: f32, yaw_rad: f32, wz: f32, speed: f32) { + // Update display speed with EMA + self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; + if self.v_disp < 3.0 / 3.6 { + // < 3 km/h (below walking speed, treat as stationary for display) + self.v_disp = 0.0; + } + + // Transform earth-frame acceleration to car-frame + let (a_lon_ms2, a_lat_ms2) = earth_to_car(ax_earth, ay_earth, yaw_rad); + + // Convert to g units + let a_lon = a_lon_ms2 / G; + let a_lat = a_lat_ms2 / G; + + // Update EMA filters for ALL values (critical for rejecting road bump noise) + let alpha = self.config.alpha; + self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; + self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; + self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; + + // Independent state detection for each mode component + // Modes can be combined (e.g., ACCEL+CORNER, BRAKE+CORNER) + + // If stopped, clear all modes + if speed < self.config.min_speed { + self.mode.clear(); + return; + } + + // Check cornering (independent of accel/brake) + let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr + && self.yaw_ema.abs() > self.config.yaw_thr + && (self.a_lat_ema * self.yaw_ema) > 0.0; + + let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit + && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; + + if self.mode.has_corner() { + // Exit corner if conditions no longer met + if cornering_exit { + self.mode.set_corner(false); + } + } else { + // Enter corner if conditions met + if cornering_active { + self.mode.set_corner(true); + } + } + + // Check acceleration (mutually exclusive with braking) + // Uses filtered a_lon_ema to reject road bump noise + if self.mode.has_accel() { + // Exit if acceleration dropped + if self.a_lon_ema < self.config.acc_exit { + self.mode.set_accel(false); + } + } else if !self.mode.has_brake() { + // Enter accel if not braking and threshold met + if self.a_lon_ema > self.config.acc_thr { + self.mode.set_accel(true); + } + } + + // Check braking (mutually exclusive with acceleration) + // Uses filtered a_lon_ema to reject road bump noise + if self.mode.has_brake() { + // Exit if braking force dropped + if self.a_lon_ema > self.config.brake_exit { + self.mode.set_brake(false); + } + } else if !self.mode.has_accel() { + // Enter brake if not accelerating and threshold met + if self.a_lon_ema < self.config.brake_thr { + self.mode.set_brake(true); + } + } + } + + /// Get current mode + pub fn get_mode(&self) -> &Mode { + &self.mode + } + + /// Get current mode as u8 for telemetry + pub fn get_mode_u8(&self) -> u8 { + self.mode.as_u8() + } + + /// Get display speed in km/h + pub fn get_speed_kmh(&self) -> f32 { + self.v_disp * 3.6 + } + + /// Force speed to zero (for ZUPT) + pub fn reset_speed(&mut self) { + self.v_disp = 0.0; + } + + /// Check if display speed is near zero + pub fn is_stopped(&self) -> bool { + self.v_disp < 0.1 // < 0.36 km/h + } + + /// Get EMA filtered lateral acceleration (for diagnostics) + #[allow(dead_code)] + pub fn get_a_lat_ema(&self) -> f32 { + self.a_lat_ema + } + + /// Get EMA filtered yaw rate (for diagnostics) + #[allow(dead_code)] + pub fn get_yaw_ema(&self) -> f32 { + self.yaw_ema + } + + /// Update configuration (e.g., from MQTT) + pub fn update_config(&mut self, config: ModeConfig) { + self.config = config; + } + + /// Update mode using pre-blended hybrid acceleration + /// + /// This is the preferred method when using SensorFusion, as it receives + /// longitudinal acceleration that's already blended from GPS and IMU sources, + /// and centripetal lateral acceleration (speed × yaw_rate) for corner detection. + /// + /// # Arguments + /// * `a_lon_blended` - Blended longitudinal acceleration (m/s², vehicle frame) + /// * `a_lat_centripetal` - Centripetal lateral acceleration (m/s², = speed × yaw_rate) + /// * `wz` - Yaw rate (rad/s) + /// * `speed` - Vehicle speed (m/s) + pub fn update_hybrid(&mut self, a_lon_blended: f32, a_lat_centripetal: f32, wz: f32, speed: f32) { + // Update display speed with EMA + self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; + if self.v_disp < 3.0 / 3.6 { + self.v_disp = 0.0; + } + + // Convert to g units (accelerations are in m/s²) + let a_lon = a_lon_blended / G; + let a_lat = a_lat_centripetal / G; + + // Update EMA filters + // Note: longitudinal already filtered/blended, but still apply EMA for consistency + let alpha = self.config.alpha; + self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; + self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; + self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; + + // If stopped, clear all modes + if speed < self.config.min_speed { + self.mode.clear(); + return; + } + + // Check cornering (independent of accel/brake) + let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr + && self.yaw_ema.abs() > self.config.yaw_thr + && (self.a_lat_ema * self.yaw_ema) > 0.0; + + let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit + && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; + + if self.mode.has_corner() { + if cornering_exit { + self.mode.set_corner(false); + } + } else if cornering_active { + self.mode.set_corner(true); + } + + // Check acceleration (mutually exclusive with braking) + if self.mode.has_accel() { + if self.a_lon_ema < self.config.acc_exit { + self.mode.set_accel(false); + } + } else if !self.mode.has_brake() && self.a_lon_ema > self.config.acc_thr { + self.mode.set_accel(true); + } + + // Check braking (mutually exclusive with acceleration) + if self.mode.has_brake() { + if self.a_lon_ema > self.config.brake_exit { + self.mode.set_brake(false); + } + } else if !self.mode.has_accel() && self.a_lon_ema < self.config.brake_thr { + self.mode.set_brake(true); + } + } + + /// Get EMA filtered longitudinal acceleration (for diagnostics) + pub fn get_a_lon_ema(&self) -> f32 { + self.a_lon_ema + } +} + +impl Default for ModeClassifier { + fn default() -> Self { + Self::new() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_mode_idle_by_default() { + let classifier = ModeClassifier::new(); + assert!(classifier.get_mode().is_idle()); + } + + #[test] + fn test_acceleration_detection() { + let mut classifier = ModeClassifier::new(); + + // Simulate forward acceleration at 5 m/s (above min_speed of 2.0) + let ax = 0.25 * G; // 0.25g forward + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed = 5.0; + + // Multiple updates needed for EMA to converge (alpha=0.25) + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_accel()); + } + + #[test] + fn test_braking_detection() { + let mut classifier = ModeClassifier::new(); + + // Simulate braking at 5 m/s (above min_speed of 2.0) + let ax = -0.3 * G; // 0.3g deceleration + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed = 5.0; + + // Multiple updates needed for EMA to converge + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_brake()); + } + + #[test] + fn test_no_mode_when_stopped() { + let mut classifier = ModeClassifier::new(); + + // Even with high acceleration, should stay IDLE when speed is 0 + let ax = -0.5 * G; // 0.5g deceleration (would trigger brake) + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed = 0.0; // STOPPED + + classifier.update(ax, ay, yaw, wz, speed); + assert!(classifier.get_mode().is_idle()); // Must stay IDLE when stopped + } + + #[test] + fn test_combined_accel_corner() { + let mut classifier = ModeClassifier::new(); + + // Simulate acceleration while cornering (corner exit) + let ax = 0.20 * G; // 0.20g forward acceleration + let ay = 0.15 * G; // 0.15g lateral + let yaw = 0.0; + let wz = 0.08; // 0.08 rad/s yaw rate + let speed = 8.0; // 8 m/s + + // Multiple updates for EMA convergence + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + + // Should detect both acceleration and cornering + assert!( + classifier.get_mode().has_accel(), + "Should detect acceleration" + ); + assert!( + classifier.get_mode().has_corner(), + "Should detect cornering" + ); + assert_eq!(classifier.get_mode().as_str(), "ACCEL+CORNER"); + } + + #[test] + fn test_combined_brake_corner() { + let mut classifier = ModeClassifier::new(); + + // Simulate braking while cornering (corner entry) + let ax = -0.25 * G; // 0.25g braking + let ay = -0.15 * G; // 0.15g lateral (left turn) + let yaw = 0.0; + let wz = -0.08; // -0.08 rad/s yaw rate (left) + let speed = 8.0; // 8 m/s + + // Multiple updates for EMA convergence + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + + // Should detect both braking and cornering + assert!(classifier.get_mode().has_brake(), "Should detect braking"); + assert!( + classifier.get_mode().has_corner(), + "Should detect cornering" + ); + assert_eq!(classifier.get_mode().as_str(), "BRAKE+CORNER"); + } + + #[test] + fn test_hysteresis_prevents_oscillation() { + let mut classifier = ModeClassifier::new(); + + let speed = 5.0; + let yaw = 0.0; + let wz = 0.0; + let ay = 0.0; + + // Acceleration above entry threshold - multiple updates for EMA + let ax1 = 0.20 * G; // Use higher value to ensure EMA exceeds threshold + for _ in 0..10 { + classifier.update(ax1, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_accel(), "Should enter ACCEL"); + + // Drop to 0.06g (above 0.05g exit threshold) - EMA should stay above exit + let ax2 = 0.06 * G; + for _ in 0..5 { + classifier.update(ax2, ay, yaw, wz, speed); + } + assert!( + classifier.get_mode().has_accel(), + "Should stay ACCEL (EMA still above exit)" + ); + + // Drop to 0.00g - EMA will gradually fall below exit threshold + let ax3 = 0.00 * G; + for _ in 0..20 { + classifier.update(ax3, ay, yaw, wz, speed); + } + assert!( + classifier.get_mode().is_idle(), + "Should exit to IDLE (EMA below exit)" + ); + } + + #[test] + fn test_speed_threshold_prevents_false_modes() { + let mut classifier = ModeClassifier::new(); + + // High acceleration but speed below threshold + let ax = 0.30 * G; + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed_low = 1.5; // Below 2.0 m/s threshold + + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed_low); + } + assert!( + classifier.get_mode().is_idle(), + "Should stay IDLE when speed < min_speed" + ); + + // Same acceleration, speed above threshold + let speed_high = 2.5; + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed_high); + } + assert!( + classifier.get_mode().has_accel(), + "Should detect ACCEL when speed > min_speed" + ); + } + + #[test] + fn test_corner_independent_persistence() { + let mut classifier = ModeClassifier::new(); + + // Start cornering - multiple updates for EMA + let ax = 0.0; + let ay = 0.15 * G; + let yaw = 0.0; + let wz = 0.08; + let speed = 8.0; + + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_corner(), "Should detect corner"); + + // Add acceleration while still cornering + let ax2 = 0.20 * G; + for _ in 0..10 { + classifier.update(ax2, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_accel(), "Should detect accel"); + assert!( + classifier.get_mode().has_corner(), + "Should still detect corner" + ); + assert_eq!(classifier.get_mode().as_u8(), 5, "Should be ACCEL+CORNER"); + + // Stop accelerating but keep cornering - EMA needs to drop + let ax3 = 0.0; + for _ in 0..20 { + classifier.update(ax3, ay, yaw, wz, speed); + } + assert!(!classifier.get_mode().has_accel(), "Should exit accel"); + assert!(classifier.get_mode().has_corner(), "Should still corner"); + } + + #[test] + fn test_default_alpha_uses_constant() { + // This test ensures ModeConfig::default() uses the shared constant + // Prevents bugs where alpha is hardcoded in multiple places with different values + let config = ModeConfig::default(); + assert_eq!( + config.alpha, DEFAULT_MODE_ALPHA, + "ModeConfig::default() must use DEFAULT_MODE_ALPHA constant" + ); + } +} diff --git a/framework/src/transforms.rs b/framework/src/transforms.rs index 90fbe7f..fef1cdb 100644 --- a/framework/src/transforms.rs +++ b/framework/src/transforms.rs @@ -80,7 +80,6 @@ pub fn remove_gravity( /// /// # Returns /// * `(a_lon, a_lat)` - Longitudinal and lateral acceleration (m/s²) -#[allow(dead_code)] pub fn earth_to_car(ax_earth: f32, ay_earth: f32, yaw_rad: f32) -> (f32, f32) { let cos_yaw = yaw_rad.cos(); let sin_yaw = yaw_rad.sin(); diff --git a/sensors/blackbox/src/filter.rs b/sensors/blackbox/src/filter.rs index aba8e7d..90fe6f6 100644 --- a/sensors/blackbox/src/filter.rs +++ b/sensors/blackbox/src/filter.rs @@ -1,423 +1,8 @@ -/// Biquad IIR low-pass filter for vibration removal -/// -/// Implements a 2nd-order Butterworth low-pass filter optimized for -/// removing engine/road vibration (10-100Hz) while preserving -/// driving dynamics (0-3Hz). -/// -/// Design: Butterworth (maximally flat passband, Q = 0.707) -/// Typical cutoff: 4Hz at 200Hz sample rate - -#[derive(Clone)] -pub struct BiquadFilter { - // Filter coefficients (normalized) - b0: f32, - b1: f32, - b2: f32, - a1: f32, - a2: f32, - - // Filter state (previous samples) - x1: f32, // x[n-1] - x2: f32, // x[n-2] - y1: f32, // y[n-1] - y2: f32, // y[n-2] -} - -impl BiquadFilter { - /// Create a new Butterworth low-pass filter - /// - /// # Arguments - /// * `cutoff_hz` - Cutoff frequency in Hz (e.g., 4.0) - /// * `sample_rate_hz` - Sample rate in Hz (e.g., 200.0) - /// - /// # Panics - /// Panics if cutoff >= sample_rate/2 (Nyquist limit) - pub fn new_lowpass(cutoff_hz: f32, sample_rate_hz: f32) -> Self { - assert!( - cutoff_hz < sample_rate_hz / 2.0, - "Cutoff must be below Nyquist frequency" - ); - - // Butterworth Q factor for critically damped response - let q = core::f32::consts::FRAC_1_SQRT_2; // 0.7071... - - // Pre-warp the cutoff frequency for bilinear transform - let omega = 2.0 * core::f32::consts::PI * cutoff_hz / sample_rate_hz; - let sin_omega = omega.sin(); - let cos_omega = omega.cos(); - let alpha = sin_omega / (2.0 * q); - - // Compute coefficients (lowpass) - let b0 = (1.0 - cos_omega) / 2.0; - let b1 = 1.0 - cos_omega; - let b2 = (1.0 - cos_omega) / 2.0; - let a0 = 1.0 + alpha; - let a1 = -2.0 * cos_omega; - let a2 = 1.0 - alpha; - - // Normalize by a0 - Self { - b0: b0 / a0, - b1: b1 / a0, - b2: b2 / a0, - a1: a1 / a0, - a2: a2 / a0, - x1: 0.0, - x2: 0.0, - y1: 0.0, - y2: 0.0, - } - } - - /// Process a single sample through the filter - /// - /// # Arguments - /// * `input` - Input sample - /// - /// # Returns - /// Filtered output sample - pub fn process(&mut self, input: f32) -> f32 { - // Direct Form I implementation - // y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] - let output = self.b0 * input + self.b1 * self.x1 + self.b2 * self.x2 - - self.a1 * self.y1 - - self.a2 * self.y2; - - // Update state - self.x2 = self.x1; - self.x1 = input; - self.y2 = self.y1; - self.y1 = output; - - output - } - - /// Initialize filter state to a steady value - /// - /// Use this to avoid startup transients when you know - /// the initial value (e.g., after calibration shows ~0) - pub fn reset_to(&mut self, value: f32) { - self.x1 = value; - self.x2 = value; - self.y1 = value; - self.y2 = value; - } -} - -#[cfg(test)] -mod tests { - use super::*; - - // Test with IMU-rate parameters (200Hz sampling, 4Hz cutoff) - const SAMPLE_RATE: f32 = 200.0; - const CUTOFF: f32 = 4.0; - - // Test with telemetry-rate parameters (20Hz sampling) - const TELEM_RATE: f32 = 20.0; - const TELEM_CUTOFF_SLOW: f32 = 2.0; // Conservative cutoff for testing - const TELEM_CUTOFF_FAST: f32 = 5.0; // Actual config used in fusion.rs - - #[test] - fn test_filter_creation() { - let filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); - // Verify coefficients are reasonable - assert!(filter.b0 > 0.0 && filter.b0 < 1.0); - assert!(filter.b1 > 0.0); - assert!(filter.b2 > 0.0 && filter.b2 < 1.0); - } - - #[test] - #[should_panic(expected = "Cutoff must be below Nyquist")] - fn test_filter_nyquist_limit() { - // Should panic: cutoff >= sample_rate/2 - let _ = BiquadFilter::new_lowpass(100.0, SAMPLE_RATE); - } - - #[test] - fn test_dc_passthrough() { - // DC signal (0 Hz) should pass through unchanged - let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); - - // Feed constant value, let filter settle - for _ in 0..100 { - filter.process(1.0); - } - - // After settling, output should be ~1.0 - let output = filter.process(1.0); - assert!( - (output - 1.0).abs() < 0.01, - "DC should pass through: got {}", - output - ); - } - - #[test] - fn test_high_frequency_attenuation() { - // 50Hz signal should be heavily attenuated (4Hz cutoff) - let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); - - // Generate 50Hz sine wave - let freq = 50.0; - let mut max_output: f32 = 0.0; - - // Let filter settle, then measure - for i in 0..1000 { - let t = i as f32 / SAMPLE_RATE; - let input = (2.0 * core::f32::consts::PI * freq * t).sin(); - let output = filter.process(input); - - if i > 500 { - // After settling - max_output = max_output.max(output.abs()); - } - } - - // At 50Hz with 4Hz cutoff, attenuation should be significant - // 50Hz is ~3.6 octaves above 4Hz, expect -40dB or more - // That's amplitude ratio of 0.01 or less - assert!( - max_output < 0.05, - "50Hz should be attenuated: got peak {}", - max_output - ); - } - - #[test] - fn test_low_frequency_passthrough() { - // 1Hz signal should pass through mostly unchanged - let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); - - let freq = 1.0; - let mut max_output: f32 = 0.0; - - for i in 0..2000 { - // Need longer for 1Hz signal - let t = i as f32 / SAMPLE_RATE; - let input = (2.0 * core::f32::consts::PI * freq * t).sin(); - let output = filter.process(input); - - if i > 1000 { - max_output = max_output.max(output.abs()); - } - } - - // 1Hz is well below 4Hz cutoff, should pass with minimal attenuation - assert!( - max_output > 0.9, - "1Hz should pass through: got peak {}", - max_output - ); - } - - #[test] - fn test_step_response_no_overshoot() { - // Butterworth should have no overshoot (critically damped) - let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); - - let mut max_output: f32 = 0.0; - - // Step from 0 to 1 - for _ in 0..500 { - let output = filter.process(1.0); - max_output = max_output.max(output); - } - - // Should never exceed 1.0 (no overshoot) - assert!( - max_output <= 1.01, - "Should not overshoot: got max {}", - max_output - ); - } - - #[test] - fn test_reset_to_value() { - let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); - - // Reset to steady state of 0.5 - filter.reset_to(0.5); - - // Immediately processing 0.5 should output ~0.5 - let output = filter.process(0.5); - assert!( - (output - 0.5).abs() < 0.01, - "After reset_to, should output steady value: got {}", - output - ); - } - - #[test] - fn test_vibration_simulation() { - // Simulate realistic scenario: driving signal + engine vibration - let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); - - let driving_freq = 0.5; // 0.5 Hz - slow braking - let vibration_freq = 60.0; // 60 Hz - engine vibration - let driving_amplitude = 0.3; // 0.3g braking - let vibration_amplitude = 0.05; // 0.05g vibration - - let mut driving_energy = 0.0; - let mut vibration_energy = 0.0; - let mut output_energy = 0.0; - - for i in 0..4000 { - // 20 seconds - let t = i as f32 / SAMPLE_RATE; - - // Input: driving signal + vibration noise - let driving = driving_amplitude * (2.0 * core::f32::consts::PI * driving_freq * t).sin(); - let vibration = - vibration_amplitude * (2.0 * core::f32::consts::PI * vibration_freq * t).sin(); - let input = driving + vibration; - - let output = filter.process(input); - - if i > 1000 { - // After settling - driving_energy += driving * driving; - vibration_energy += vibration * vibration; - output_energy += output * output; - } - } - - // Output energy should be close to driving energy (vibration removed) - let ratio = output_energy / driving_energy; - assert!( - ratio > 0.8 && ratio < 1.2, - "Filter should preserve driving signal: energy ratio = {}", - ratio - ); - - // Output should have much less energy than input (vibration removed) - let input_energy = driving_energy + vibration_energy; - assert!( - output_energy < input_energy * 0.95, - "Filter should remove vibration energy" - ); - } - - #[test] - fn test_telemetry_rate_filter_conservative() { - // Test at telemetry rate with conservative 2Hz cutoff - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_SLOW, TELEM_RATE); - - // DC should pass through - for _ in 0..50 { - filter.process(1.0); - } - let dc_output = filter.process(1.0); - assert!( - (dc_output - 1.0).abs() < 0.02, - "DC should pass at telemetry rate: got {}", - dc_output - ); - - // Test low frequency (0.5 Hz driving input) - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_SLOW, TELEM_RATE); - let freq = 0.5; // 0.5 Hz - typical braking/accel - let mut max_output: f32 = 0.0; - - for i in 0..200 { - let t = i as f32 / TELEM_RATE; - let input = (2.0 * core::f32::consts::PI * freq * t).sin(); - let output = filter.process(input); - - if i > 100 { - max_output = max_output.max(output.abs()); - } - } - - // 0.5 Hz is well below 2Hz cutoff, should pass through - assert!( - max_output > 0.85, - "0.5Hz should pass at 20Hz/2Hz filter: got {}", - max_output - ); - - // Test high frequency rejection (8Hz - at Nyquist, should be attenuated) - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_SLOW, TELEM_RATE); - let freq = 8.0; - let mut max_output: f32 = 0.0; - - for i in 0..200 { - let t = i as f32 / TELEM_RATE; - let input = (2.0 * core::f32::consts::PI * freq * t).sin(); - let output = filter.process(input); - - if i > 100 { - max_output = max_output.max(output.abs()); - } - } - - // 8Hz is well above 2Hz cutoff at 20Hz sample rate - assert!( - max_output < 0.2, - "8Hz should be attenuated at 20Hz/2Hz filter: got {}", - max_output - ); - } - - #[test] - fn test_telemetry_rate_filter_actual_config() { - // Test with actual fusion.rs config: 20Hz sampling, 5Hz cutoff - // This is faster/more responsive than conservative 2Hz - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_FAST, TELEM_RATE); - - // DC should pass through - for _ in 0..50 { - filter.process(1.0); - } - let dc_output = filter.process(1.0); - assert!( - (dc_output - 1.0).abs() < 0.02, - "DC should pass at 5Hz cutoff: got {}", - dc_output - ); - - // Test driving frequency (1 Hz - typical braking/accel) - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_FAST, TELEM_RATE); - let freq = 1.0; - let mut max_output: f32 = 0.0; - - for i in 0..200 { - let t = i as f32 / TELEM_RATE; - let input = (2.0 * core::f32::consts::PI * freq * t).sin(); - let output = filter.process(input); - - if i > 100 { - max_output = max_output.max(output.abs()); - } - } - - // 1 Hz is well below 5Hz cutoff, should pass through with minimal attenuation - assert!( - max_output > 0.9, - "1Hz should pass at 20Hz/5Hz filter: got {}", - max_output - ); - - // Test vibration rejection (8Hz - should be attenuated even at 5Hz cutoff) - let mut filter = BiquadFilter::new_lowpass(TELEM_CUTOFF_FAST, TELEM_RATE); - let freq = 8.0; - let mut max_output: f32 = 0.0; - - for i in 0..200 { - let t = i as f32 / TELEM_RATE; - let input = (2.0 * core::f32::consts::PI * freq * t).sin(); - let output = filter.process(input); - - if i > 100 { - max_output = max_output.max(output.abs()); - } - } - - // 8Hz is above 5Hz cutoff, should be attenuated (but less than at 2Hz cutoff) - assert!( - max_output < 0.5, - "8Hz should be attenuated at 20Hz/5Hz filter: got {}", - max_output - ); - } -} - +//! Signal filtering module +//! +//! Re-exports from the sensor-fusion framework crate. +//! The actual implementation lives in `sensor_fusion::filter`. +//! +//! Note: Currently unused in production - kept for potential future use. + +pub use sensor_fusion::filter::*; diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs index 53d22bc..b919f7a 100644 --- a/sensors/blackbox/src/fusion.rs +++ b/sensors/blackbox/src/fusion.rs @@ -1,799 +1,6 @@ -//! Sensor Fusion Module +//! Sensor fusion module //! -//! This module handles: -//! 1. GPS-derived longitudinal acceleration (from velocity changes) -//! 2. GPS/IMU acceleration blending with adaptive weights -//! 3. Dynamic tilt offset learning (ZUPT enhancement) -//! 4. Moving gravity estimation (continuous calibration while driving) -//! -//! The goal is to provide clean, drift-free acceleration for mode detection -//! that works regardless of device mounting angle. - -// Sensor fusion for GPS/IMU blending -// Biquad filter removed from longitudinal path for ~50% faster response. -// GPS blend provides primary smoothing, mode.rs EMA handles residual noise. - -/// Configuration for sensor fusion -#[derive(Clone, Copy)] -pub struct FusionConfig { - /// GPS rate threshold for high-confidence GPS (Hz) - pub gps_high_rate: f32, - /// GPS rate threshold for medium-confidence GPS (Hz) - pub gps_medium_rate: f32, - /// Maximum age of GPS data before considered stale (seconds) - pub gps_max_age: f32, - /// GPS weight when rate >= high_rate (0.0-1.0) - /// Higher = more GPS, smoother but slower response - /// Lower = more IMU, faster response but potential drift - pub gps_weight_high: f32, - /// GPS weight when rate >= medium_rate (0.0-1.0) - pub gps_weight_medium: f32, - /// GPS weight when rate < medium_rate (0.0-1.0) - pub gps_weight_low: f32, - /// Time required at stop to learn tilt offset (seconds) - pub tilt_learn_time: f32, - /// Speed threshold for steady-state detection (m/s) - pub steady_state_speed_tolerance: f32, - /// Yaw rate threshold for steady-state (rad/s) - pub steady_state_yaw_tolerance: f32, - /// Time required in steady state to update gravity estimate (seconds) - pub gravity_learn_time: f32, - /// Alpha for gravity estimate update (smaller = slower, more stable) - pub gravity_alpha: f32, - // Note: lon_filter_cutoff and sample_rate removed - no longer using Biquad filter - // GPS blend provides primary smoothing, mode.rs EMA handles residual noise -} - -impl Default for FusionConfig { - /// Default configuration balanced for city/highway/canyon driving. - /// - /// GPS weights are moderate (70/50/30) for balanced responsiveness: - /// - Fast enough for canyon driving (~80-100ms latency) - /// - Smooth enough for city/highway (no jitter) - /// - /// For track use, consider increasing GPS weights (90/70/50) for - /// maximum accuracy at the cost of some latency. - fn default() -> Self { - Self { - gps_high_rate: 20.0, - gps_medium_rate: 10.0, - gps_max_age: 0.2, // 200ms - // Balanced blend: 70% GPS when fresh and fast - // Gives ~80-100ms latency instead of ~120-150ms - gps_weight_high: 0.70, // 70% GPS / 30% IMU at >= 20Hz - gps_weight_medium: 0.50, // 50% GPS / 50% IMU at >= 10Hz - gps_weight_low: 0.30, // 30% GPS / 70% IMU at < 10Hz - tilt_learn_time: 3.0, - steady_state_speed_tolerance: 0.5, // 0.5 m/s = 1.8 km/h - steady_state_yaw_tolerance: 0.087, // ~5 deg/s - gravity_learn_time: 2.0, - gravity_alpha: 0.02, // Very slow update - } - } -} - -/// GPS-derived acceleration tracker -pub struct GpsAcceleration { - /// Previous speed (m/s) - prev_speed: f32, - /// Previous timestamp (seconds, monotonic) - prev_time: f32, - /// Computed longitudinal acceleration (m/s²) - accel_lon: f32, - /// Is the acceleration estimate valid? - valid: bool, - /// Current GPS rate estimate (Hz) - gps_rate: f32, - /// Time since last valid GPS fix (seconds) - time_since_fix: f32, - /// EMA for GPS rate calculation - rate_ema: f32, - /// Last fix count for rate calculation - last_fix_count: u32, - /// Time of last rate calculation - last_rate_time: f32, -} - -impl GpsAcceleration { - pub fn new() -> Self { - Self { - prev_speed: 0.0, - prev_time: 0.0, - accel_lon: 0.0, - valid: false, - gps_rate: 0.0, - time_since_fix: 999.0, - rate_ema: 0.0, - last_fix_count: 0, - last_rate_time: 0.0, - } - } - - /// Update with new GPS speed measurement - /// - /// # Arguments - /// * `speed` - Current speed in m/s - /// * `time` - Current timestamp in seconds (monotonic) - /// - /// # Returns - /// The computed longitudinal acceleration, or None if not enough data - pub fn update(&mut self, speed: f32, time: f32) -> Option { - self.time_since_fix = 0.0; - - if self.prev_time > 0.0 { - let dt = time - self.prev_time; - - // Validate dt: should be 20-200ms for 5-50Hz GPS - if dt > 0.02 && dt < 0.5 { - self.accel_lon = (speed - self.prev_speed) / dt; - self.valid = true; - - self.prev_speed = speed; - self.prev_time = time; - - return Some(self.accel_lon); - } - } - - self.prev_speed = speed; - self.prev_time = time; - None - } - - /// Update GPS rate estimate - /// - /// # Arguments - /// * `fix_count` - Total number of valid fixes received - /// * `time` - Current timestamp in seconds - pub fn update_rate(&mut self, fix_count: u32, time: f32) { - if self.last_rate_time > 0.0 { - let dt = time - self.last_rate_time; - if dt >= 1.0 { - let fixes = fix_count.saturating_sub(self.last_fix_count); - let instant_rate = fixes as f32 / dt; - - // EMA smoothing for rate - self.rate_ema = 0.3 * instant_rate + 0.7 * self.rate_ema; - self.gps_rate = self.rate_ema; - - self.last_fix_count = fix_count; - self.last_rate_time = time; - } - } else { - self.last_fix_count = fix_count; - self.last_rate_time = time; - } - } - - /// Advance time without GPS fix (for staleness tracking) - pub fn advance_time(&mut self, dt: f32) { - self.time_since_fix += dt; - if self.time_since_fix > 0.5 { - self.valid = false; - } - } - - /// Get current GPS-derived acceleration - pub fn get_accel(&self) -> Option { - if self.valid { - Some(self.accel_lon) - } else { - None - } - } - - /// Get current GPS rate estimate (Hz) - pub fn get_rate(&self) -> f32 { - self.gps_rate - } - - /// Check if GPS data is fresh - pub fn is_fresh(&self, max_age: f32) -> bool { - self.time_since_fix < max_age - } -} - -impl Default for GpsAcceleration { - fn default() -> Self { - Self::new() - } -} - -/// Tilt offset estimator - learns mounting offset when stopped -pub struct TiltEstimator { - /// Accumulated X acceleration (earth frame) - accel_x_sum: f32, - /// Accumulated Y acceleration (earth frame) - accel_y_sum: f32, - /// Number of samples accumulated - sample_count: u32, - /// Time stationary (seconds) - stationary_time: f32, - /// Learned tilt offset X (m/s²) - offset_x: f32, - /// Learned tilt offset Y (m/s²) - offset_y: f32, - /// Is the offset valid (learned at least once)? - offset_valid: bool, - /// Required stationary time to learn (seconds) - learn_time: f32, -} - -impl TiltEstimator { - pub fn new(learn_time: f32) -> Self { - Self { - accel_x_sum: 0.0, - accel_y_sum: 0.0, - sample_count: 0, - stationary_time: 0.0, - offset_x: 0.0, - offset_y: 0.0, - offset_valid: false, - learn_time, - } - } - - /// Update with acceleration sample while stationary - /// - /// # Arguments - /// * `ax_earth` - X acceleration in earth frame (m/s²) - /// * `ay_earth` - Y acceleration in earth frame (m/s²) - /// * `dt` - Time step (seconds) - /// - /// # Returns - /// true if offset was just updated (can trigger visual feedback) - pub fn update_stationary(&mut self, ax_earth: f32, ay_earth: f32, dt: f32) -> bool { - self.stationary_time += dt; - self.accel_x_sum += ax_earth; - self.accel_y_sum += ay_earth; - self.sample_count += 1; - - // After sufficient time, compute offset - if self.stationary_time >= self.learn_time && self.sample_count > 50 { - let new_offset_x = self.accel_x_sum / self.sample_count as f32; - let new_offset_y = self.accel_y_sum / self.sample_count as f32; - - // Only update if change is significant (avoid jitter) - let change = ((new_offset_x - self.offset_x).powi(2) - + (new_offset_y - self.offset_y).powi(2)) - .sqrt(); - - if change > 0.05 || !self.offset_valid { - self.offset_x = new_offset_x; - self.offset_y = new_offset_y; - self.offset_valid = true; - - // Reset accumulators for continued learning - self.accel_x_sum = 0.0; - self.accel_y_sum = 0.0; - self.sample_count = 0; - - return true; - } - } - - false - } - - /// Reset stationary tracking (called when vehicle starts moving) - pub fn reset_stationary(&mut self) { - self.accel_x_sum = 0.0; - self.accel_y_sum = 0.0; - self.sample_count = 0; - self.stationary_time = 0.0; - } - - /// Apply tilt correction to acceleration - pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { - if self.offset_valid { - (ax_earth - self.offset_x, ay_earth - self.offset_y) - } else { - (ax_earth, ay_earth) - } - } -} - -/// Moving gravity estimator - learns gravity offset while driving -pub struct GravityEstimator { - /// Estimated gravity offset X (earth frame, m/s²) - offset_x: f32, - /// Estimated gravity offset Y (earth frame, m/s²) - offset_y: f32, - /// Is the estimate valid? - valid: bool, - /// Speed history for steady-state detection - speed_history: [f32; 10], - speed_idx: usize, - /// Time in steady state (seconds) - steady_time: f32, - /// Configuration - speed_tolerance: f32, - yaw_tolerance: f32, - learn_time: f32, - alpha: f32, -} - -impl GravityEstimator { - pub fn new(config: &FusionConfig) -> Self { - Self { - offset_x: 0.0, - offset_y: 0.0, - valid: false, - speed_history: [0.0; 10], - speed_idx: 0, - steady_time: 0.0, - speed_tolerance: config.steady_state_speed_tolerance, - yaw_tolerance: config.steady_state_yaw_tolerance, - learn_time: config.gravity_learn_time, - alpha: config.gravity_alpha, - } - } - - /// Update with current driving state - /// - /// # Arguments - /// * `ax_earth` - X acceleration (earth frame, m/s²) - /// * `ay_earth` - Y acceleration (earth frame, m/s²) - /// * `speed` - Current speed (m/s) - /// * `yaw_rate` - Current yaw rate (rad/s) - /// * `dt` - Time step (seconds) - pub fn update( - &mut self, - ax_earth: f32, - ay_earth: f32, - speed: f32, - yaw_rate: f32, - dt: f32, - ) -> bool { - // Update speed history - self.speed_history[self.speed_idx] = speed; - self.speed_idx = (self.speed_idx + 1) % self.speed_history.len(); - - // Check if in steady state (constant velocity) - let speed_stable = self.is_speed_stable(); - let yaw_low = yaw_rate.abs() < self.yaw_tolerance; - let moving = speed > 2.0; // Must be moving (not stopped) - - if speed_stable && yaw_low && moving { - self.steady_time += dt; - - if self.steady_time >= self.learn_time { - // In steady state, expected acceleration is ~0 - // Any measured acceleration is gravity/mounting error - self.offset_x = (1.0 - self.alpha) * self.offset_x + self.alpha * ax_earth; - self.offset_y = (1.0 - self.alpha) * self.offset_y + self.alpha * ay_earth; - self.valid = true; - return true; - } - } else { - self.steady_time = 0.0; - } - - false - } - - fn is_speed_stable(&self) -> bool { - if self.speed_history[0] < 1.0 { - return false; // Need some speed data - } - - let mean: f32 = self.speed_history.iter().sum::() / self.speed_history.len() as f32; - let max_dev = self - .speed_history - .iter() - .map(|&s| (s - mean).abs()) - .fold(0.0f32, |a, b| a.max(b)); - - max_dev < self.speed_tolerance - } - - /// Apply gravity correction to acceleration - pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { - if self.valid { - (ax_earth - self.offset_x, ay_earth - self.offset_y) - } else { - (ax_earth, ay_earth) - } - } -} - -/// Main sensor fusion processor -pub struct SensorFusion { - pub config: FusionConfig, - /// GPS-derived acceleration - pub gps_accel: GpsAcceleration, - /// Tilt estimator (learns when stopped) - pub tilt_estimator: TiltEstimator, - /// Gravity estimator (learns while driving) - pub gravity_estimator: GravityEstimator, - /// Blended longitudinal acceleration (m/s²) - blended_lon: f32, - /// Raw IMU longitudinal for display (m/s², vehicle frame) - lon_imu_raw: f32, - /// Corrected lateral acceleration for display (m/s², vehicle frame) - lat_corrected: f32, - /// Centripetal lateral acceleration for mode detection (m/s²) - lat_centripetal: f32, - /// Last GPS blend weight (for diagnostics) - last_gps_weight: f32, - /// Was stationary last update? - was_stationary: bool, -} - -impl SensorFusion { - pub fn new(config: FusionConfig) -> Self { - Self { - gps_accel: GpsAcceleration::new(), - tilt_estimator: TiltEstimator::new(config.tilt_learn_time), - gravity_estimator: GravityEstimator::new(&config), - blended_lon: 0.0, - lon_imu_raw: 0.0, - lat_corrected: 0.0, - lat_centripetal: 0.0, - last_gps_weight: 0.0, - was_stationary: false, - config, - } - } - - /// Process IMU data (call at telemetry rate, e.g., 20Hz) - /// - /// # Arguments - /// * `ax_earth` - X acceleration in earth frame (m/s²) - /// * `ay_earth` - Y acceleration in earth frame (m/s²) - /// * `yaw` - Vehicle yaw angle (rad, for earth-to-car transform) - /// * `speed` - Vehicle speed (m/s) - /// * `yaw_rate` - Yaw rate (rad/s) - /// * `dt` - Time step (seconds) - /// * `is_stationary` - Whether vehicle is currently stationary - /// - /// # Returns - /// (lon_blended, lat_centripetal) - Accelerations for mode detection (m/s²) - /// lon_blended: GPS/IMU blended longitudinal - /// lat_centripetal: speed * yaw_rate (pro-style, mount-independent) - #[allow(clippy::too_many_arguments)] - pub fn process_imu( - &mut self, - ax_earth: f32, - ay_earth: f32, - yaw: f32, - speed: f32, - yaw_rate: f32, - dt: f32, - is_stationary: bool, - ) -> (f32, f32) { - // Track GPS staleness - self.gps_accel.advance_time(dt); - - // Apply tilt correction (learned when stopped) - let (ax_tilt, ay_tilt) = self.tilt_estimator.correct(ax_earth, ay_earth); - - // Apply gravity correction (learned while driving) - let (ax_corr, ay_corr) = self.gravity_estimator.correct(ax_tilt, ay_tilt); - - // Transform to vehicle frame - let (lon_imu_raw, lat_imu_raw) = earth_to_car(ax_corr, ay_corr, yaw); - - // Store raw IMU longitudinal for display/fallback - // No Biquad filter - GPS blend provides primary smoothing, - // mode.rs EMA handles residual noise. This gives ~50% faster response. - self.lon_imu_raw = lon_imu_raw; - - // Store corrected lateral for display - self.lat_corrected = lat_imu_raw; - - // Calculate centripetal lateral for mode detection (pro-style) - // a_lateral = v * omega (mount-angle independent, no filtering needed) - self.lat_centripetal = speed * yaw_rate; - - // Handle stationary state transitions - if is_stationary { - // Learn tilt offset when stopped - self.tilt_estimator - .update_stationary(ax_earth, ay_earth, dt); - } else { - if self.was_stationary { - // Just started moving - self.tilt_estimator.reset_stationary(); - } - - // Update gravity estimate while driving - self.gravity_estimator - .update(ax_earth, ay_earth, speed, yaw_rate, dt); - } - self.was_stationary = is_stationary; - - // Blend GPS and IMU for longitudinal acceleration - // GPS provides smooth, drift-free acceleration from velocity changes - // IMU provides fast response but has vibration noise - // Blending gives best of both: smooth when GPS available, fast fallback - let gps_weight = self.compute_gps_weight(); - self.last_gps_weight = gps_weight; - - let lon_blended = if let Some(gps_lon) = self.gps_accel.get_accel() { - gps_weight * gps_lon + (1.0 - gps_weight) * self.lon_imu_raw - } else { - self.lon_imu_raw - }; - - self.blended_lon = lon_blended; - - // Return blended longitudinal and centripetal lateral for mode detection - (lon_blended, self.lat_centripetal) - } - - /// Process GPS speed update (call at GPS rate, e.g., 25Hz) - /// - /// # Arguments - /// * `speed` - GPS speed in m/s - /// * `time` - Monotonic timestamp in seconds - pub fn process_gps(&mut self, speed: f32, time: f32) { - self.gps_accel.update(speed, time); - } - - /// Update GPS rate estimate (call periodically, e.g., every second) - pub fn update_gps_rate(&mut self, fix_count: u32, time: f32) { - self.gps_accel.update_rate(fix_count, time); - } - - /// Get blended longitudinal acceleration (vehicle frame, m/s²) - /// This is GPS/IMU blended - same value used by mode classification - /// Positive = forward acceleration - pub fn get_lon_blended(&self) -> f32 { - self.blended_lon - } - - /// Get corrected lateral acceleration for display (vehicle frame, m/s²) - /// This is accelerometer-based with tilt/gravity correction - /// Positive = left - pub fn get_lat_display(&self) -> f32 { - self.lat_corrected - } - - /// Compute GPS weight based on GPS rate and data freshness - fn compute_gps_weight(&self) -> f32 { - let rate = self.gps_accel.get_rate(); - let fresh = self.gps_accel.is_fresh(self.config.gps_max_age); - - if !fresh { - return 0.0; - } - - // Blending based on GPS rate using configurable weights - if rate >= self.config.gps_high_rate { - self.config.gps_weight_high - } else if rate >= self.config.gps_medium_rate { - self.config.gps_weight_medium - } else if rate > 0.0 { - self.config.gps_weight_low - } else { - 0.0 - } - } -} - -impl Default for SensorFusion { - fn default() -> Self { - Self::new(FusionConfig::default()) - } -} - -/// Earth to car frame transformation -/// -/// Converts earth-frame accelerations to vehicle-frame (longitudinal/lateral) -/// -/// # Arguments -/// * `ax_earth` - X acceleration in earth frame (m/s²) -/// * `ay_earth` - Y acceleration in earth frame (m/s²) -/// * `yaw` - Vehicle heading (rad, 0 = East, π/2 = North) -/// -/// # Returns -/// (lon, lat) - Longitudinal (forward+) and lateral (left+) accelerations -fn earth_to_car(ax_earth: f32, ay_earth: f32, yaw: f32) -> (f32, f32) { - let cos_yaw = yaw.cos(); - let sin_yaw = yaw.sin(); - - // Rotate from earth to car frame - let lon = ax_earth * cos_yaw + ay_earth * sin_yaw; - let lat = -ax_earth * sin_yaw + ay_earth * cos_yaw; - - (lon, lat) -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_gps_acceleration_basic() { - let mut gps = GpsAcceleration::new(); - - // First update - no acceleration yet - assert!(gps.update(10.0, 0.0).is_none()); - - // Second update - should compute acceleration - let accel = gps.update(12.0, 0.1); // 2 m/s increase over 0.1s = 20 m/s² - assert!(accel.is_some()); - assert!((accel.unwrap() - 20.0).abs() < 0.1); - } - - #[test] - fn test_gps_acceleration_decel() { - let mut gps = GpsAcceleration::new(); - - gps.update(20.0, 0.0); - let accel = gps.update(15.0, 0.1); // 5 m/s decrease = -50 m/s² - - assert!(accel.is_some()); - assert!((accel.unwrap() + 50.0).abs() < 0.1); - } - - #[test] - fn test_gps_staleness() { - let mut gps = GpsAcceleration::new(); - let config = FusionConfig::default(); - - gps.update(10.0, 0.0); - gps.update(12.0, 0.1); - - assert!(gps.is_fresh(config.gps_max_age)); - - // Advance time without GPS update - gps.advance_time(0.3); // > 200ms - assert!(!gps.is_fresh(config.gps_max_age)); - assert!(gps.get_accel().is_none()); - } - - #[test] - fn test_tilt_estimator_learns_and_corrects() { - let mut tilt = TiltEstimator::new(1.0); // 1 second learn time - - // Simulate stationary with 0.5 m/s² offset (tilted mount) - for _ in 0..300 { - // 1.5 seconds at 200Hz - tilt.update_stationary(0.5, 0.3, 0.005); - } - - // Correction should now reduce the offset to near zero - let (ax, ay) = tilt.correct(0.5, 0.3); - assert!(ax.abs() < 0.1, "Corrected X should be ~0: {}", ax); - assert!(ay.abs() < 0.1, "Corrected Y should be ~0: {}", ay); - } - - #[test] - fn test_centripetal_lateral_during_turn() { - // Simulate a turn: speed = 10 m/s, yaw_rate = 0.3 rad/s - // Expected centripetal: 10 * 0.3 = 3 m/s² - let config = FusionConfig::default(); - let mut fusion = SensorFusion::new(config); - - // Process with turn conditions - let (_lon, lat) = fusion.process_imu( - 0.0, // No earth-frame X accel - 0.0, // No earth-frame Y accel - 0.0, // Heading - 10.0, // Speed = 10 m/s - 0.3, // Yaw rate = 0.3 rad/s (turning) - 0.05, // dt - false, // Not stationary - ); - - // Lateral should be centripetal: speed * yaw_rate = 3 m/s² - assert!( - (lat - 3.0).abs() < 0.01, - "Centripetal lateral should be 3.0: got {}", - lat - ); - } - - #[test] - fn test_lateral_returns_to_zero_after_turn() { - // This test verifies the critical bug fix: - // After a turn ends, lateral should return to zero immediately - // (not "stick" due to earth-frame filtering) - - let config = FusionConfig::default(); - let mut fusion = SensorFusion::new(config); - - // Simulate a 90° turn - let mut yaw = 0.0; - for i in 0..20 { - // 1 second of turning at 20Hz - let yaw_rate = 1.57; // ~90°/s - yaw += yaw_rate * 0.05; - - let (_lon, lat) = fusion.process_imu( - 0.0, // Earth X - 0.0, // Earth Y - yaw, // Changing heading - 10.0, // Constant speed - yaw_rate, // Turning - 0.05, // dt - false, // Not stationary - ); - - // During turn, lateral should be positive (speed * positive yaw_rate) - if i > 5 { - assert!( - lat > 10.0, - "During turn, lateral should be high: {}", - lat - ); - } - } - - // Turn ends - yaw_rate goes to zero - let (_lon, lat) = fusion.process_imu( - 0.0, // Earth X - 0.0, // Earth Y - yaw, // Final heading - 10.0, // Still moving - 0.0, // NO MORE TURNING - 0.05, // dt - false, - ); - - // Lateral should be zero immediately (no sticking!) - assert!( - lat.abs() < 0.01, - "After turn ends, lateral should be ~0 immediately: got {}", - lat - ); - } - - #[test] - fn test_display_matches_mode_classification_longitudinal() { - // Verify that get_lon_blended() returns the same value as process_imu() - let config = FusionConfig::default(); - let mut fusion = SensorFusion::new(config); - - // Setup GPS - fusion.gps_accel.rate_ema = 25.0; - fusion.gps_accel.update(10.0, 0.0); - fusion.gps_accel.update(12.0, 0.04); - - let (lon_from_process, _lat) = fusion.process_imu(5.0, 3.0, 0.0, 10.0, 0.0, 0.05, false); - - let lon_for_display = fusion.get_lon_blended(); - - assert_eq!( - lon_from_process, lon_for_display, - "Display lon must match mode classifier input" - ); - } - - #[test] - fn test_earth_to_car_transform() { - // Heading East (yaw = 0) - let (lon, lat) = earth_to_car(10.0, 0.0, 0.0); - assert!((lon - 10.0).abs() < 0.01); - assert!(lat.abs() < 0.01); - - // Heading North (yaw = π/2) - let (lon, lat) = earth_to_car(0.0, 10.0, core::f32::consts::FRAC_PI_2); - assert!((lon - 10.0).abs() < 0.01); - assert!(lat.abs() < 0.01); - - // Heading West (yaw = π) - let (lon, lat) = earth_to_car(-10.0, 0.0, core::f32::consts::PI); - assert!((lon - 10.0).abs() < 0.01); - assert!(lat.abs() < 0.01); - } - - #[test] - fn test_gps_rate_calculation() { - let mut gps = GpsAcceleration::new(); - - // Initialize - gps.update_rate(0, 0.0); - - // After 1 second with 25 fixes - gps.update_rate(25, 1.0); - assert!((gps.get_rate() - 7.5).abs() < 1.0); // First update, EMA starts low +//! Re-exports from the sensor-fusion framework crate. +//! The actual implementation lives in `sensor_fusion::fusion`. - // After another second with 25 more fixes - gps.update_rate(50, 2.0); - let rate = gps.get_rate(); - assert!(rate > 10.0 && rate < 30.0, "Rate should converge: {}", rate); - } -} +pub use sensor_fusion::fusion::*; diff --git a/sensors/blackbox/src/mode.rs b/sensors/blackbox/src/mode.rs index 24bbbfa..8c62a47 100644 --- a/sensors/blackbox/src/mode.rs +++ b/sensors/blackbox/src/mode.rs @@ -1,605 +1,6 @@ -#![allow(dead_code)] // API methods for future use +//! Mode detection module +//! +//! Re-exports from the sensor-fusion framework crate. +//! The actual implementation lives in `sensor_fusion::mode`. -/// Driving mode classifier -/// Detects IDLE, ACCEL, BRAKE, CORNER, ACCEL+CORNER, and BRAKE+CORNER modes -/// based on acceleration and yaw rate -/// -/// ## Hybrid Acceleration Support -/// -/// This classifier can operate in two modes: -/// 1. **Legacy mode**: Uses `update()` with raw earth-frame IMU accelerations -/// 2. **Hybrid mode**: Uses `update_hybrid()` with pre-blended longitudinal acceleration -/// -/// Hybrid mode receives longitudinal acceleration that's already blended from -/// GPS-derived (from velocity changes) and IMU sources. This provides: -/// - Drift-free longitudinal detection (GPS doesn't drift) -/// - Mounting angle independence (GPS measures actual vehicle acceleration) -/// - Graceful fallback when GPS is unavailable -use sensor_fusion::transforms::earth_to_car; - -const G: f32 = 9.80665; // m/s² - -/// Driving mode flags - can be combined -pub struct Mode { - flags: u8, -} - -// Mode bit flags -const IDLE: u8 = 0; -const ACCEL: u8 = 1; -const BRAKE: u8 = 2; -const CORNER: u8 = 4; - -impl Mode { - pub fn idle() -> Self { - Self { flags: IDLE } - } - - pub fn is_idle(&self) -> bool { - self.flags == IDLE - } - - pub fn has_accel(&self) -> bool { - self.flags & ACCEL != 0 - } - - pub fn has_brake(&self) -> bool { - self.flags & BRAKE != 0 - } - - pub fn has_corner(&self) -> bool { - self.flags & CORNER != 0 - } - - pub fn set_accel(&mut self, enabled: bool) { - if enabled { - self.flags |= ACCEL; - self.flags &= !BRAKE; // Clear brake (mutually exclusive) - } else { - self.flags &= !ACCEL; - } - } - - pub fn set_brake(&mut self, enabled: bool) { - if enabled { - self.flags |= BRAKE; - self.flags &= !ACCEL; // Clear accel (mutually exclusive) - } else { - self.flags &= !BRAKE; - } - } - - pub fn set_corner(&mut self, enabled: bool) { - if enabled { - self.flags |= CORNER; - } else { - self.flags &= !CORNER; - } - } - - pub fn clear(&mut self) { - self.flags = IDLE; - } - - /// Get mode as string for display - pub fn as_str(&self) -> &'static str { - match self.flags { - 0 => "IDLE", - 1 => "ACCEL", - 2 => "BRAKE", - 4 => "CORNER", - 5 => "ACCEL+CORNER", - 6 => "BRAKE+CORNER", - _ => "UNKNOWN", - } - } - - /// Get mode as u8 for binary telemetry - pub fn as_u8(&self) -> u8 { - self.flags - } -} - -impl Default for Mode { - fn default() -> Self { - Self::idle() - } -} - -/// Default EMA alpha for mode detection smoothing -/// τ ≈ 72ms at 20Hz processing rate -/// Used by both ModeConfig::default() and settings handler in main.rs -pub const DEFAULT_MODE_ALPHA: f32 = 0.50; - -/// Configurable thresholds for mode detection -#[derive(Debug, Clone, Copy)] -pub struct ModeConfig { - pub min_speed: f32, // m/s (minimum speed for maneuvers) - pub acc_thr: f32, // g (acceleration threshold) - pub acc_exit: f32, // g (acceleration exit threshold) - pub brake_thr: f32, // g (braking threshold, negative) - pub brake_exit: f32, // g (braking exit threshold, negative) - pub lat_thr: f32, // g (lateral acceleration threshold) - pub lat_exit: f32, // g (lateral acceleration exit threshold) - pub yaw_thr: f32, // rad/s (yaw rate threshold for cornering) - pub alpha: f32, // EMA smoothing factor -} - -impl Default for ModeConfig { - fn default() -> Self { - Self { - min_speed: 2.0, // ~7 km/h - must be moving for accel/brake/corner - acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) - acc_exit: 0.05, // exit when below 0.05g - brake_thr: -0.18, // -0.18g - city driving (normal braking) - brake_exit: -0.09, // exit when above -0.09g - lat_thr: 0.12, // 0.12g lateral - city turns - lat_exit: 0.06, // exit when below 0.06g - yaw_thr: 0.05, // ~2.9°/s yaw rate - alpha: DEFAULT_MODE_ALPHA, - } - } -} - -/// Mode classifier with EMA filtering -pub struct ModeClassifier { - pub mode: Mode, - pub config: ModeConfig, - - // EMA filtered values (all accelerations filtered to reject road bumps) - a_lon_ema: f32, - a_lat_ema: f32, - yaw_ema: f32, - - // Display speed with separate smoothing - v_disp: f32, - v_alpha: f32, // Speed display smoothing factor -} - -impl ModeClassifier { - pub fn new() -> Self { - Self { - mode: Mode::default(), - config: ModeConfig::default(), - a_lon_ema: 0.0, - a_lat_ema: 0.0, - yaw_ema: 0.0, - v_disp: 0.0, - v_alpha: 0.85, // High alpha = fast response to GPS speed changes - } - } - - /// Update mode based on current vehicle state - /// - /// # Arguments - /// * `ax_earth` - Longitudinal acceleration in earth frame (m/s²) - /// * `ay_earth` - Lateral acceleration in earth frame (m/s²) - /// * `yaw_rad` - Vehicle yaw angle (rad) - /// * `wz` - Yaw rate (rad/s) - /// * `speed` - Vehicle speed (m/s) - pub fn update(&mut self, ax_earth: f32, ay_earth: f32, yaw_rad: f32, wz: f32, speed: f32) { - // Update display speed with EMA - self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; - if self.v_disp < 3.0 / 3.6 { - // < 3 km/h (below walking speed, treat as stationary for display) - self.v_disp = 0.0; - } - - // Transform earth-frame acceleration to car-frame - let (a_lon_ms2, a_lat_ms2) = earth_to_car(ax_earth, ay_earth, yaw_rad); - - // Convert to g units - let a_lon = a_lon_ms2 / G; - let a_lat = a_lat_ms2 / G; - - // Update EMA filters for ALL values (critical for rejecting road bump noise) - let alpha = self.config.alpha; - self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; - self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; - self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; - - // Independent state detection for each mode component - // Modes can be combined (e.g., ACCEL+CORNER, BRAKE+CORNER) - - // If stopped, clear all modes - if speed < self.config.min_speed { - self.mode.clear(); - return; - } - - // Check cornering (independent of accel/brake) - let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr - && self.yaw_ema.abs() > self.config.yaw_thr - && (self.a_lat_ema * self.yaw_ema) > 0.0; - - let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit - && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; - - if self.mode.has_corner() { - // Exit corner if conditions no longer met - if cornering_exit { - self.mode.set_corner(false); - } - } else { - // Enter corner if conditions met - if cornering_active { - self.mode.set_corner(true); - } - } - - // Check acceleration (mutually exclusive with braking) - // Uses filtered a_lon_ema to reject road bump noise - if self.mode.has_accel() { - // Exit if acceleration dropped - if self.a_lon_ema < self.config.acc_exit { - self.mode.set_accel(false); - } - } else if !self.mode.has_brake() { - // Enter accel if not braking and threshold met - if self.a_lon_ema > self.config.acc_thr { - self.mode.set_accel(true); - } - } - - // Check braking (mutually exclusive with acceleration) - // Uses filtered a_lon_ema to reject road bump noise - if self.mode.has_brake() { - // Exit if braking force dropped - if self.a_lon_ema > self.config.brake_exit { - self.mode.set_brake(false); - } - } else if !self.mode.has_accel() { - // Enter brake if not accelerating and threshold met - if self.a_lon_ema < self.config.brake_thr { - self.mode.set_brake(true); - } - } - } - - /// Get current mode - pub fn get_mode(&self) -> &Mode { - &self.mode - } - - /// Get current mode as u8 for telemetry - pub fn get_mode_u8(&self) -> u8 { - self.mode.as_u8() - } - - /// Get display speed in km/h - pub fn get_speed_kmh(&self) -> f32 { - self.v_disp * 3.6 - } - - /// Force speed to zero (for ZUPT) - pub fn reset_speed(&mut self) { - self.v_disp = 0.0; - } - - /// Check if display speed is near zero - pub fn is_stopped(&self) -> bool { - self.v_disp < 0.1 // < 0.36 km/h - } - - /// Get EMA filtered lateral acceleration (for diagnostics) - #[allow(dead_code)] - pub fn get_a_lat_ema(&self) -> f32 { - self.a_lat_ema - } - - /// Get EMA filtered yaw rate (for diagnostics) - #[allow(dead_code)] - pub fn get_yaw_ema(&self) -> f32 { - self.yaw_ema - } - - /// Update configuration (e.g., from MQTT) - pub fn update_config(&mut self, config: ModeConfig) { - self.config = config; - } - - /// Update mode using pre-blended hybrid acceleration - /// - /// This is the preferred method when using SensorFusion, as it receives - /// longitudinal acceleration that's already blended from GPS and IMU sources, - /// and centripetal lateral acceleration (speed × yaw_rate) for corner detection. - /// - /// # Arguments - /// * `a_lon_blended` - Blended longitudinal acceleration (m/s², vehicle frame) - /// * `a_lat_centripetal` - Centripetal lateral acceleration (m/s², = speed × yaw_rate) - /// * `wz` - Yaw rate (rad/s) - /// * `speed` - Vehicle speed (m/s) - pub fn update_hybrid(&mut self, a_lon_blended: f32, a_lat_centripetal: f32, wz: f32, speed: f32) { - // Update display speed with EMA - self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; - if self.v_disp < 3.0 / 3.6 { - self.v_disp = 0.0; - } - - // Convert to g units (accelerations are in m/s²) - let a_lon = a_lon_blended / G; - let a_lat = a_lat_centripetal / G; - - // Update EMA filters - // Note: longitudinal already filtered/blended, but still apply EMA for consistency - let alpha = self.config.alpha; - self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; - self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; - self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; - - // If stopped, clear all modes - if speed < self.config.min_speed { - self.mode.clear(); - return; - } - - // Check cornering (independent of accel/brake) - let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr - && self.yaw_ema.abs() > self.config.yaw_thr - && (self.a_lat_ema * self.yaw_ema) > 0.0; - - let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit - && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; - - if self.mode.has_corner() { - if cornering_exit { - self.mode.set_corner(false); - } - } else if cornering_active { - self.mode.set_corner(true); - } - - // Check acceleration (mutually exclusive with braking) - if self.mode.has_accel() { - if self.a_lon_ema < self.config.acc_exit { - self.mode.set_accel(false); - } - } else if !self.mode.has_brake() && self.a_lon_ema > self.config.acc_thr { - self.mode.set_accel(true); - } - - // Check braking (mutually exclusive with acceleration) - if self.mode.has_brake() { - if self.a_lon_ema > self.config.brake_exit { - self.mode.set_brake(false); - } - } else if !self.mode.has_accel() && self.a_lon_ema < self.config.brake_thr { - self.mode.set_brake(true); - } - } - - /// Get EMA filtered longitudinal acceleration (for diagnostics) - pub fn get_a_lon_ema(&self) -> f32 { - self.a_lon_ema - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_mode_idle_by_default() { - let classifier = ModeClassifier::new(); - assert!(classifier.get_mode().is_idle()); - } - - #[test] - fn test_acceleration_detection() { - let mut classifier = ModeClassifier::new(); - - // Simulate forward acceleration at 5 m/s (above min_speed of 2.0) - let ax = 0.25 * G; // 0.25g forward - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed = 5.0; - - // Multiple updates needed for EMA to converge (alpha=0.25) - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_accel()); - } - - #[test] - fn test_braking_detection() { - let mut classifier = ModeClassifier::new(); - - // Simulate braking at 5 m/s (above min_speed of 2.0) - let ax = -0.3 * G; // 0.3g deceleration - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed = 5.0; - - // Multiple updates needed for EMA to converge - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_brake()); - } - - #[test] - fn test_no_mode_when_stopped() { - let mut classifier = ModeClassifier::new(); - - // Even with high acceleration, should stay IDLE when speed is 0 - let ax = -0.5 * G; // 0.5g deceleration (would trigger brake) - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed = 0.0; // STOPPED - - classifier.update(ax, ay, yaw, wz, speed); - assert!(classifier.get_mode().is_idle()); // Must stay IDLE when stopped - } - - #[test] - fn test_combined_accel_corner() { - let mut classifier = ModeClassifier::new(); - - // Simulate acceleration while cornering (corner exit) - let ax = 0.20 * G; // 0.20g forward acceleration - let ay = 0.15 * G; // 0.15g lateral - let yaw = 0.0; - let wz = 0.08; // 0.08 rad/s yaw rate - let speed = 8.0; // 8 m/s - - // Multiple updates for EMA convergence - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - - // Should detect both acceleration and cornering - assert!( - classifier.get_mode().has_accel(), - "Should detect acceleration" - ); - assert!( - classifier.get_mode().has_corner(), - "Should detect cornering" - ); - assert_eq!(classifier.get_mode().as_str(), "ACCEL+CORNER"); - } - - #[test] - fn test_combined_brake_corner() { - let mut classifier = ModeClassifier::new(); - - // Simulate braking while cornering (corner entry) - let ax = -0.25 * G; // 0.25g braking - let ay = -0.15 * G; // 0.15g lateral (left turn) - let yaw = 0.0; - let wz = -0.08; // -0.08 rad/s yaw rate (left) - let speed = 8.0; // 8 m/s - - // Multiple updates for EMA convergence - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - - // Should detect both braking and cornering - assert!(classifier.get_mode().has_brake(), "Should detect braking"); - assert!( - classifier.get_mode().has_corner(), - "Should detect cornering" - ); - assert_eq!(classifier.get_mode().as_str(), "BRAKE+CORNER"); - } - - #[test] - fn test_hysteresis_prevents_oscillation() { - let mut classifier = ModeClassifier::new(); - - let speed = 5.0; - let yaw = 0.0; - let wz = 0.0; - let ay = 0.0; - - // Acceleration above entry threshold - multiple updates for EMA - let ax1 = 0.20 * G; // Use higher value to ensure EMA exceeds threshold - for _ in 0..10 { - classifier.update(ax1, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_accel(), "Should enter ACCEL"); - - // Drop to 0.06g (above 0.05g exit threshold) - EMA should stay above exit - let ax2 = 0.06 * G; - for _ in 0..5 { - classifier.update(ax2, ay, yaw, wz, speed); - } - assert!( - classifier.get_mode().has_accel(), - "Should stay ACCEL (EMA still above exit)" - ); - - // Drop to 0.00g - EMA will gradually fall below exit threshold - let ax3 = 0.00 * G; - for _ in 0..20 { - classifier.update(ax3, ay, yaw, wz, speed); - } - assert!( - classifier.get_mode().is_idle(), - "Should exit to IDLE (EMA below exit)" - ); - } - - #[test] - fn test_speed_threshold_prevents_false_modes() { - let mut classifier = ModeClassifier::new(); - - // High acceleration but speed below threshold - let ax = 0.30 * G; - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed_low = 1.5; // Below 2.0 m/s threshold - - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed_low); - } - assert!( - classifier.get_mode().is_idle(), - "Should stay IDLE when speed < min_speed" - ); - - // Same acceleration, speed above threshold - let speed_high = 2.5; - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed_high); - } - assert!( - classifier.get_mode().has_accel(), - "Should detect ACCEL when speed > min_speed" - ); - } - - #[test] - fn test_corner_independent_persistence() { - let mut classifier = ModeClassifier::new(); - - // Start cornering - multiple updates for EMA - let ax = 0.0; - let ay = 0.15 * G; - let yaw = 0.0; - let wz = 0.08; - let speed = 8.0; - - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_corner(), "Should detect corner"); - - // Add acceleration while still cornering - let ax2 = 0.20 * G; - for _ in 0..10 { - classifier.update(ax2, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_accel(), "Should detect accel"); - assert!( - classifier.get_mode().has_corner(), - "Should still detect corner" - ); - assert_eq!(classifier.get_mode().as_u8(), 5, "Should be ACCEL+CORNER"); - - // Stop accelerating but keep cornering - EMA needs to drop - let ax3 = 0.0; - for _ in 0..20 { - classifier.update(ax3, ay, yaw, wz, speed); - } - assert!(!classifier.get_mode().has_accel(), "Should exit accel"); - assert!(classifier.get_mode().has_corner(), "Should still corner"); - } - - #[test] - fn test_default_alpha_uses_constant() { - // This test ensures ModeConfig::default() uses the shared constant - // Prevents bugs where alpha is hardcoded in multiple places with different values - let config = ModeConfig::default(); - assert_eq!( - config.alpha, DEFAULT_MODE_ALPHA, - "ModeConfig::default() must use DEFAULT_MODE_ALPHA constant" - ); - } -} +pub use sensor_fusion::mode::*; From f62b55f4f61c3dbb3c3f46f7d4ee5438fa1226ae Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sun, 18 Jan 2026 08:36:41 -0800 Subject: [PATCH 12/30] Add Biquad vibration filter and centripetal lateral MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add 5Hz Butterworth low-pass filter to IMU longitudinal acceleration to remove engine vibration (20-100Hz) while preserving driving dynamics - Based on ArduPilot research: accelerometer outer loop uses 10Hz filter - Add YawRateCalibrator to learn gyro bias during straight-line driving - Switch lateral G to centripetal calculation (speed × yaw_rate) for mount-independent, instant response without sticking - Add tests for vibration attenuation and dynamics passthrough - Update CLAUDE.md with filter theory and data flow diagrams --- CLAUDE.md | 80 ++++-- framework/src/filter.rs | 9 +- framework/src/fusion.rs | 467 +++++++++++++++++++++++++++++++-- framework/src/lib.rs | 5 +- sensors/blackbox/src/main.rs | 10 + sensors/blackbox/src/system.rs | 11 +- 6 files changed, 534 insertions(+), 48 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 0385081..ffe8499 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -560,17 +560,30 @@ Combined states: ### filter.rs - Biquad Low-Pass Filter -2nd-order Butterworth IIR filter for vibration removal. +2nd-order Butterworth IIR filter for vibration removal from IMU longitudinal acceleration. -**Purpose:** -- Remove engine vibration (30-100 Hz) -- Remove road texture noise (5-20 Hz) -- Preserve driving dynamics (0-2 Hz) +**Problem:** +Engine vibration (20-100 Hz) causes 0.08-0.12g noise on longitudinal G readings when +the vehicle is running but stationary. This makes mode detection unreliable. + +**Solution (based on ArduPilot research):** +- ArduPilot uses 10Hz low-pass on accelerometers for outer control loops +- Academic research shows 1-5Hz Butterworth effective for vehicle dynamics +- Physics: driving events (braking, acceleration, cornering) are < 3Hz +- Engine/road vibration is 20-100Hz → easily separable from driving dynamics **Configuration:** - Sample rate: 20 Hz (telemetry rate) -- Cutoff: 2 Hz -- Q = 0.707 (critically damped, no overshoot) +- Cutoff: 5 Hz (preserves all driving dynamics, removes vibration) +- Q = 0.707 (Butterworth - maximally flat passband, no overshoot) + +**Frequency Response:** +``` +0-3 Hz: ~100% pass (driving dynamics preserved) +5 Hz: -3 dB (cutoff point) +10 Hz: -12 dB (attenuated) +30+ Hz: -30+ dB (engine vibration removed) +``` **Implementation:** ```rust @@ -601,11 +614,18 @@ Handles GPS/IMU blending, tilt correction, and continuous calibration. - Slowly updates gravity estimate (α=0.02, ~50 second convergence) - Essential for track/canyon driving where stops are rare -4. **SensorFusion** - Main processor +4. **YawRateCalibrator** - Learns gyro bias while driving straight + - When GPS heading is stable (±0.5°/s for 2+ seconds), assumes true yaw rate ≈ 0 + - Any measured yaw rate during this time is gyro bias + - Applies correction to prevent lateral drift in centripetal calculation + - Essential for highway driving where ZUPT rarely triggers + +5. **SensorFusion** - Main processor - Applies tilt correction - Applies gravity correction - - Low-pass filters to remove vibration + - **5 Hz Butterworth low-pass filter** removes engine vibration (see filter.rs) - Blends GPS and IMU for longitudinal acceleration + - Computes centripetal lateral (speed × calibrated_yaw_rate) **GPS/IMU Blending Ratios (configurable in FusionConfig):** ``` @@ -620,20 +640,34 @@ For track use, consider increasing GPS weights for maximum accuracy. **Data Flow:** ``` -GPS (25Hz) → GpsAcceleration → lon_accel_gps - ↓ -IMU → remove_gravity → body_to_earth → TiltCorrect → GravityCorrect → Biquad Filter - ↓ - lon_accel_imu - ↓ - Blend(gps, imu) → lon_blended - ↓ - ┌───────────────────────┴───────────────────────┐ - ↓ ↓ - mode.rs (classification) Dashboard (G-meter) -``` - -**Dashboard Display:** The G-meter shows the same blended longitudinal and filtered lateral +LONGITUDINAL: +GPS (25Hz) → GpsAcceleration → lon_accel_gps ────┐ + │ +IMU → remove_gravity → body_to_earth → Tilt → Gravity → Biquad(5Hz) → lon_accel_imu + │ + Blend(gps, imu) ◄┘ + ↓ + lon_blended + +LATERAL (centripetal): +GPS heading → YawRateCalibrator → learned_bias + ↓ +Gyro yaw_rate → (yaw_rate - bias) → calibrated_yaw_rate + ↓ +GPS speed × calibrated_yaw_rate → lat_centripetal + +OUTPUT: +(lon_blended, lat_centripetal) → mode.rs (classification) + → Dashboard (G-meter) +``` + +**Why Centripetal Lateral?** +- Traditional IMU lateral (accelerometer) suffers from mount angle sensitivity and can "stick" +- Centripetal: `a_lat = v × ω` is physics-based, instant, and mount-independent +- Uses calibrated yaw rate to prevent drift during highway driving +- Pro telemetry systems use this approach + +**Dashboard Display:** The G-meter shows the same blended longitudinal and centripetal lateral values that mode classification uses. What you see is what the algorithm sees. ### websocket_server.rs - HTTP Dashboard Server diff --git a/framework/src/filter.rs b/framework/src/filter.rs index e46fbd5..abda75d 100644 --- a/framework/src/filter.rs +++ b/framework/src/filter.rs @@ -1,8 +1,13 @@ //! Signal filtering utilities //! //! Contains Biquad IIR filter implementation for vibration removal. -//! Note: Currently unused in production - kept for potential future use -//! and to verify the math with unit tests. +//! Used by SensorFusion to remove engine vibration (20-100Hz) from +//! IMU longitudinal acceleration while preserving driving dynamics (0-3Hz). +//! +//! Based on research: +//! - ArduPilot uses 10Hz low-pass on accelerometers (outer loop) +//! - Academic papers show 1-5Hz Butterworth effective for vehicle dynamics +//! - Physics: driving events (braking, acceleration, cornering) are < 3Hz /// Biquad IIR low-pass filter for vibration removal /// diff --git a/framework/src/fusion.rs b/framework/src/fusion.rs index 323795f..273e076 100644 --- a/framework/src/fusion.rs +++ b/framework/src/fusion.rs @@ -3,16 +3,26 @@ //! This module handles: //! 1. GPS-derived longitudinal acceleration (from velocity changes) //! 2. GPS/IMU acceleration blending with adaptive weights -//! 3. Dynamic tilt offset learning (ZUPT enhancement) -//! 4. Moving gravity estimation (continuous calibration while driving) +//! 3. Butterworth low-pass filtering to remove engine vibration +//! 4. Dynamic tilt offset learning (ZUPT enhancement) +//! 5. Moving gravity estimation (continuous calibration while driving) +//! 6. GPS heading-based yaw rate calibration //! //! The goal is to provide clean, drift-free acceleration for mode detection //! that works regardless of device mounting angle. +//! +//! ## Vibration Filtering +//! +//! Engine vibration (20-100+ Hz) is removed using a 2nd-order Butterworth +//! low-pass filter at 5 Hz. This preserves driving dynamics (0-3 Hz) while +//! eliminating noise from engine, alternator, road surface, etc. +//! +//! Research references: +//! - ArduPilot uses 10 Hz on accelerometers (outer loop doesn't need fast response) +//! - Academic research shows 1-5 Hz Butterworth is effective for vehicle dynamics +//! - Driving events (braking, acceleration, cornering) are all below 3 Hz -// Sensor fusion for GPS/IMU blending -// Biquad filter removed from longitudinal path for ~50% faster response. -// GPS blend provides primary smoothing, mode.rs EMA handles residual noise. - +use crate::filter::BiquadFilter; use crate::transforms::earth_to_car; /// Configuration for sensor fusion @@ -42,8 +52,12 @@ pub struct FusionConfig { pub gravity_learn_time: f32, /// Alpha for gravity estimate update (smaller = slower, more stable) pub gravity_alpha: f32, - // Note: lon_filter_cutoff and sample_rate removed - no longer using Biquad filter - // GPS blend provides primary smoothing, mode.rs EMA handles residual noise + /// Low-pass filter cutoff for IMU longitudinal (Hz) + /// Removes engine vibration (20-100Hz) while passing driving dynamics (0-3Hz) + /// Research: ArduPilot uses 10Hz, academic papers suggest 1-5Hz for vehicle dynamics + pub lon_filter_cutoff: f32, + /// Sample rate for longitudinal filter (Hz) - must match telemetry rate + pub lon_sample_rate: f32, } impl Default for FusionConfig { @@ -53,6 +67,12 @@ impl Default for FusionConfig { /// - Fast enough for canyon driving (~80-100ms latency) /// - Smooth enough for city/highway (no jitter) /// + /// The 5 Hz Butterworth filter removes engine vibration (20-100Hz) while + /// preserving all driving dynamics (0-3Hz). This is based on: + /// - ArduPilot: uses 10Hz low-pass on accelerometers + /// - Research: shows 1-5Hz Butterworth effective for brake detection + /// - Physics: driving events (braking, cornering) are all < 3Hz + /// /// For track use, consider increasing GPS weights (90/70/50) for /// maximum accuracy at the cost of some latency. fn default() -> Self { @@ -70,6 +90,10 @@ impl Default for FusionConfig { steady_state_yaw_tolerance: 0.087, // ~5 deg/s gravity_learn_time: 2.0, gravity_alpha: 0.02, // Very slow update + // 5 Hz cutoff: passes driving dynamics (0-3Hz), removes engine vibration (20-100Hz) + // At 20 Hz sample rate, this gives ~40dB attenuation at 50Hz + lon_filter_cutoff: 5.0, + lon_sample_rate: 20.0, // Default telemetry rate } } } @@ -399,6 +423,172 @@ impl GravityEstimator { } } +/// Yaw rate calibrator using GPS heading +/// +/// When driving straight on highway (stable GPS course), the true yaw rate +/// should be ~0. Any measured yaw rate from the gyro is bias that we can +/// learn and subtract. This prevents lateral drift in centripetal calculation. +pub struct YawRateCalibrator { + /// Learned yaw rate bias (rad/s) + bias: f32, + /// Is bias estimate valid? + valid: bool, + /// Heading history for stability detection (radians) + heading_history: [f32; 20], + heading_idx: usize, + heading_count: u32, + /// Time driving straight (seconds) + straight_time: f32, + /// Accumulated yaw rate samples + yaw_sum: f32, + sample_count: u32, + /// Configuration + min_speed: f32, // m/s, minimum speed for calibration (~29 km/h) + heading_tolerance: f32, // radians, max heading deviation for "straight" (~2°) + learn_time: f32, // seconds, time required to update bias + alpha: f32, // EMA alpha for bias update +} + +impl YawRateCalibrator { + pub fn new() -> Self { + Self { + bias: 0.0, + valid: false, + heading_history: [0.0; 20], + heading_idx: 0, + heading_count: 0, + straight_time: 0.0, + yaw_sum: 0.0, + sample_count: 0, + min_speed: 8.0, // ~29 km/h + heading_tolerance: 0.035, // ~2 degrees + learn_time: 3.0, // 3 seconds of straight driving + alpha: 0.1, // Slow update for stability + } + } + + /// Update heading history with new GPS course + /// Call this at GPS rate (5-25 Hz) + pub fn update_heading(&mut self, heading: f32) { + self.heading_history[self.heading_idx] = heading; + self.heading_idx = (self.heading_idx + 1) % self.heading_history.len(); + if self.heading_count < self.heading_history.len() as u32 { + self.heading_count += 1; + } + } + + /// Update with gyro yaw rate and check for calibration opportunity + /// Call this at telemetry rate (20 Hz) + /// + /// # Returns + /// true if bias was updated + pub fn update(&mut self, yaw_rate: f32, speed: f32, dt: f32) -> bool { + // Need minimum speed and enough heading history + if speed < self.min_speed || self.heading_count < 10 { + self.reset_accumulator(); + return false; + } + + // Check if heading is stable (driving straight) + if !self.is_heading_stable() { + self.reset_accumulator(); + return false; + } + + // Accumulate yaw rate samples + self.straight_time += dt; + self.yaw_sum += yaw_rate; + self.sample_count += 1; + + // After sufficient time, learn bias + if self.straight_time >= self.learn_time && self.sample_count > 50 { + let measured_bias = self.yaw_sum / self.sample_count as f32; + + if self.valid { + // EMA update for smooth correction + self.bias = (1.0 - self.alpha) * self.bias + self.alpha * measured_bias; + } else { + self.bias = measured_bias; + self.valid = true; + } + + self.reset_accumulator(); + return true; + } + + false + } + + fn reset_accumulator(&mut self) { + self.straight_time = 0.0; + self.yaw_sum = 0.0; + self.sample_count = 0; + } + + fn is_heading_stable(&self) -> bool { + if self.heading_count < 10 { + return false; + } + + let count = self.heading_count as usize; + + // Compute circular mean (headings wrap around at ±π) + let mut sin_sum = 0.0f32; + let mut cos_sum = 0.0f32; + for &h in &self.heading_history[..count] { + sin_sum += h.sin(); + cos_sum += h.cos(); + } + let mean_heading = sin_sum.atan2(cos_sum); + + // Check all headings are within tolerance of mean + for &h in &self.heading_history[..count] { + let diff = angle_diff(h, mean_heading); + if diff.abs() > self.heading_tolerance { + return false; + } + } + + true + } + + /// Apply bias correction to yaw rate + pub fn correct(&self, yaw_rate: f32) -> f32 { + if self.valid { + yaw_rate - self.bias + } else { + yaw_rate + } + } + + /// Get current bias estimate (rad/s) + pub fn get_bias(&self) -> f32 { + self.bias + } + + /// Is bias calibration valid? + pub fn is_valid(&self) -> bool { + self.valid + } +} + +impl Default for YawRateCalibrator { + fn default() -> Self { + Self::new() + } +} + +/// Compute angular difference handling wrap-around at ±π +fn angle_diff(a: f32, b: f32) -> f32 { + let mut diff = a - b; + if diff > core::f32::consts::PI { + diff -= 2.0 * core::f32::consts::PI; + } else if diff < -core::f32::consts::PI { + diff += 2.0 * core::f32::consts::PI; + } + diff +} + /// Main sensor fusion processor pub struct SensorFusion { pub config: FusionConfig, @@ -408,10 +598,18 @@ pub struct SensorFusion { pub tilt_estimator: TiltEstimator, /// Gravity estimator (learns while driving) pub gravity_estimator: GravityEstimator, + /// Yaw rate calibrator (learns gyro bias while driving straight) + pub yaw_rate_calibrator: YawRateCalibrator, + /// Butterworth low-pass filter for IMU longitudinal (removes engine vibration) + lon_filter: BiquadFilter, /// Blended longitudinal acceleration (m/s²) blended_lon: f32, - /// Raw IMU longitudinal for display (m/s², vehicle frame) + /// Display longitudinal - GPS when fresh, otherwise filtered blended (m/s²) + lon_display: f32, + /// Raw IMU longitudinal (unfiltered, m/s², vehicle frame) lon_imu_raw: f32, + /// Filtered IMU longitudinal (m/s², vehicle frame) + lon_imu_filtered: f32, /// Corrected lateral acceleration for display (m/s², vehicle frame) lat_corrected: f32, /// Centripetal lateral acceleration for mode detection (m/s²) @@ -428,8 +626,12 @@ impl SensorFusion { gps_accel: GpsAcceleration::new(), tilt_estimator: TiltEstimator::new(config.tilt_learn_time), gravity_estimator: GravityEstimator::new(&config), + yaw_rate_calibrator: YawRateCalibrator::new(), + lon_filter: BiquadFilter::new_lowpass(config.lon_filter_cutoff, config.lon_sample_rate), blended_lon: 0.0, + lon_display: 0.0, lon_imu_raw: 0.0, + lon_imu_filtered: 0.0, lat_corrected: 0.0, lat_centripetal: 0.0, last_gps_weight: 0.0, @@ -476,17 +678,25 @@ impl SensorFusion { // Transform to vehicle frame let (lon_imu_raw, lat_imu_raw) = earth_to_car(ax_corr, ay_corr, yaw); - // Store raw IMU longitudinal for display/fallback - // No Biquad filter - GPS blend provides primary smoothing, - // mode.rs EMA handles residual noise. This gives ~50% faster response. + // Store raw IMU longitudinal (before filtering) self.lon_imu_raw = lon_imu_raw; + // Apply Butterworth low-pass filter to remove engine vibration (20-100Hz) + // Filter cutoff is 5Hz: passes driving dynamics (0-3Hz), removes vibration + // Based on ArduPilot (10Hz on accels) and research (1-5Hz for vehicle dynamics) + let lon_imu_filtered = self.lon_filter.process(lon_imu_raw); + self.lon_imu_filtered = lon_imu_filtered; + // Store corrected lateral for display self.lat_corrected = lat_imu_raw; + // Apply yaw rate calibration (removes gyro bias learned while driving straight) + let yaw_rate_corrected = self.yaw_rate_calibrator.correct(yaw_rate); + // Calculate centripetal lateral for mode detection (pro-style) // a_lateral = v * omega (mount-angle independent, no filtering needed) - self.lat_centripetal = speed * yaw_rate; + // Uses calibrated yaw rate to prevent drift on highway + self.lat_centripetal = speed * yaw_rate_corrected; // Handle stationary state transitions if is_stationary { @@ -502,24 +712,46 @@ impl SensorFusion { // Update gravity estimate while driving self.gravity_estimator .update(ax_earth, ay_earth, speed, yaw_rate, dt); + + // Update yaw rate calibrator (learns bias while driving straight) + self.yaw_rate_calibrator.update(yaw_rate, speed, dt); } self.was_stationary = is_stationary; // Blend GPS and IMU for longitudinal acceleration // GPS provides smooth, drift-free acceleration from velocity changes - // IMU provides fast response but has vibration noise - // Blending gives best of both: smooth when GPS available, fast fallback + // IMU (now filtered) provides fast response with vibration removed + // Blending gives best of both: smooth when GPS available, fast filtered fallback let gps_weight = self.compute_gps_weight(); self.last_gps_weight = gps_weight; let lon_blended = if let Some(gps_lon) = self.gps_accel.get_accel() { - gps_weight * gps_lon + (1.0 - gps_weight) * self.lon_imu_raw + // Blend GPS with filtered IMU (vibration removed) + gps_weight * gps_lon + (1.0 - gps_weight) * self.lon_imu_filtered } else { - self.lon_imu_raw + // No GPS - use filtered IMU only (still vibration-free) + self.lon_imu_filtered }; self.blended_lon = lon_blended; + // Compute display-specific longitudinal + // With Butterworth filter active, blended value is already clean + // Use GPS directly when fresh for smoothest display, otherwise use blended + let lon_for_display = if let Some(gps_lon) = self.gps_accel.get_accel() { + if self.gps_accel.is_fresh(self.config.gps_max_age) { + // GPS fresh - use it directly (smoothest) + gps_lon + } else { + // GPS stale - use filtered blended (vibration already removed by Biquad) + lon_blended + } + } else { + // No GPS accel - use filtered blended + lon_blended + }; + self.lon_display = lon_for_display; + // Return blended longitudinal and centripetal lateral for mode detection (lon_blended, self.lat_centripetal) } @@ -533,11 +765,28 @@ impl SensorFusion { self.gps_accel.update(speed, time); } + /// Process GPS heading update for yaw rate calibration + /// Call this at GPS rate when GPS is valid and speed > 0 + /// + /// # Arguments + /// * `heading` - Course over ground in radians + pub fn process_gps_heading(&mut self, heading: f32) { + self.yaw_rate_calibrator.update_heading(heading); + } + /// Update GPS rate estimate (call periodically, e.g., every second) pub fn update_gps_rate(&mut self, fix_count: u32, time: f32) { self.gps_accel.update_rate(fix_count, time); } + /// Get display-optimized longitudinal acceleration (vehicle frame, m/s²) + /// Uses GPS-derived acceleration when fresh (no vibration noise) + /// Falls back to blended with heavier smoothing when GPS stale + /// Positive = forward acceleration + pub fn get_lon_display(&self) -> f32 { + self.lon_display + } + /// Get blended longitudinal acceleration (vehicle frame, m/s²) /// This is GPS/IMU blended - same value used by mode classification /// Positive = forward acceleration @@ -548,10 +797,19 @@ impl SensorFusion { /// Get corrected lateral acceleration for display (vehicle frame, m/s²) /// This is accelerometer-based with tilt/gravity correction /// Positive = left + /// NOTE: Consider using get_lat_centripetal() instead for responsive display pub fn get_lat_display(&self) -> f32 { self.lat_corrected } + /// Get centripetal lateral acceleration (vehicle frame, m/s²) + /// Computed as speed × yaw_rate - instant, mount-independent, doesn't stick + /// This is the same value used by mode detection + /// Positive = left turn + pub fn get_lat_centripetal(&self) -> f32 { + self.lat_centripetal + } + /// Compute GPS weight based on GPS rate and data freshness fn compute_gps_weight(&self) -> f32 { let rate = self.gps_accel.get_rate(); @@ -784,4 +1042,179 @@ mod tests { // EMA: 0.3 * 25 + 0.7 * 7.5 = 7.5 + 5.25 = 12.75 assert!(rate > 10.0 && rate < 20.0, "Rate should converge: {}", rate); } + + #[test] + fn test_yaw_rate_calibrator_learns_bias() { + let mut cal = YawRateCalibrator::new(); + + // Simulate driving straight on highway with constant heading + // Heading = 0 rad (East), speed = 20 m/s, gyro has 0.01 rad/s bias + let bias = 0.01; // Small gyro bias + + // Feed stable headings (within 2° tolerance) + for _ in 0..30 { + cal.update_heading(0.0); // Constant heading + } + + // Drive straight for 4 seconds at 20 Hz with biased gyro + for _ in 0..80 { + cal.update(bias, 20.0, 0.05); + } + + // Calibrator should have learned the bias + assert!(cal.is_valid(), "Should have valid calibration after 4s straight"); + assert!( + (cal.get_bias() - bias).abs() < 0.005, + "Learned bias should be close to actual: {} vs {}", + cal.get_bias(), + bias + ); + + // Corrected yaw rate should be near zero + let corrected = cal.correct(bias); + assert!( + corrected.abs() < 0.005, + "Corrected yaw rate should be ~0: {}", + corrected + ); + } + + #[test] + fn test_yaw_rate_calibrator_rejects_turns() { + let mut cal = YawRateCalibrator::new(); + + // Feed varying headings (turning) + for i in 0..30 { + cal.update_heading(i as f32 * 0.1); // Increasing heading = turning + } + + // Try to calibrate during turn + for _ in 0..80 { + cal.update(0.1, 20.0, 0.05); + } + + // Should NOT have valid calibration because heading wasn't stable + assert!( + !cal.is_valid(), + "Should reject calibration during turns" + ); + } + + #[test] + fn test_centripetal_uses_calibrated_yaw_rate() { + // Verify that centripetal calculation uses the calibrated yaw rate + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Simulate highway driving with bias in yaw rate + let yaw_bias = 0.02; // 0.02 rad/s bias + + // Feed stable headings + for _ in 0..30 { + fusion.process_gps_heading(0.0); + } + + // Drive straight with biased yaw rate until calibration kicks in + for _ in 0..100 { + fusion.process_imu(0.0, 0.0, 0.0, 20.0, yaw_bias, 0.05, false); + } + + // Now with calibration active, centripetal should be near zero + // even though raw yaw_rate has bias + let (_lon, lat) = fusion.process_imu(0.0, 0.0, 0.0, 20.0, yaw_bias, 0.05, false); + + // Without calibration: lat = 20 * 0.02 = 0.4 m/s² + // With calibration: lat should be ~0 + assert!( + lat.abs() < 0.1, + "Calibrated lateral should be near zero on straight: {}", + lat + ); + } + + #[test] + fn test_biquad_filter_removes_engine_vibration() { + // Simulate engine vibration at 30Hz with 0.1g amplitude (~1 m/s²) + // The 5Hz Butterworth filter should attenuate this by ~30dB + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + let vibration_freq = 30.0; // Hz - typical engine vibration + let vibration_amp = 1.0; // m/s² (~0.1g) + let sample_rate = 20.0; // Hz - telemetry rate + + // Run for 2 seconds to let filter settle + let mut max_output: f32 = 0.0; + for i in 0..40 { + let t = i as f32 / sample_rate; + // Simulate vibrating IMU input in earth frame + let vibration = vibration_amp * (2.0 * core::f32::consts::PI * vibration_freq * t).sin(); + + let (lon, _lat) = fusion.process_imu( + vibration, // X earth = vibrating + 0.0, // Y earth = 0 + 0.0, // yaw = 0 (heading east) + 10.0, // speed + 0.0, // yaw_rate = 0 + 0.05, // dt = 50ms + false, // not stationary + ); + + // After settling (1 second = 20 samples), check output + if i > 20 { + max_output = max_output.max(lon.abs()); + } + } + + // 30Hz vibration should be attenuated by ~30dB at 5Hz cutoff + // That means 1 m/s² input → ~0.03 m/s² output (or less) + // Being conservative, check it's below 0.1 m/s² (~0.01g) + assert!( + max_output < 0.15, + "30Hz vibration should be heavily attenuated: got {} m/s²", + max_output + ); + } + + #[test] + fn test_biquad_filter_passes_driving_dynamics() { + // Simulate a braking event at 1Hz (typical vehicle dynamics) + // The 5Hz filter should pass this with minimal attenuation + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + let dynamics_freq = 1.0; // Hz - braking event + let dynamics_amp = 3.0; // m/s² (~0.3g braking) + let sample_rate = 20.0; // Hz + + // Run for 3 seconds + let mut max_output: f32 = 0.0; + for i in 0..60 { + let t = i as f32 / sample_rate; + let signal = dynamics_amp * (2.0 * core::f32::consts::PI * dynamics_freq * t).sin(); + + let (lon, _lat) = fusion.process_imu( + signal, + 0.0, + 0.0, + 10.0, + 0.0, + 0.05, + false, + ); + + // After settling (1 second), check output + if i > 20 { + max_output = max_output.max(lon.abs()); + } + } + + // 1Hz should pass through with >90% amplitude + // 3 m/s² input → >2.7 m/s² output + assert!( + max_output > 2.5, + "1Hz driving dynamics should pass through: got {} m/s², expected >2.5", + max_output + ); + } } diff --git a/framework/src/lib.rs b/framework/src/lib.rs index 8cfbc11..0c6a7ff 100644 --- a/framework/src/lib.rs +++ b/framework/src/lib.rs @@ -144,7 +144,10 @@ pub mod velocity; pub use mode::{Mode, ModeClassifier, ModeConfig, DEFAULT_MODE_ALPHA}; // Re-export sensor fusion types -pub use fusion::{FusionConfig, GpsAcceleration, GravityEstimator, SensorFusion, TiltEstimator}; +pub use fusion::{ + FusionConfig, GpsAcceleration, GravityEstimator, SensorFusion, TiltEstimator, + YawRateCalibrator, +}; // Re-export commonly used types from sensor_framework // Re-export EKF diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 2383b91..01cb5e4 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -470,6 +470,9 @@ fn main() { steady_state_speed_tolerance: 0.5, steady_state_yaw_tolerance: 0.087, gravity_alpha: 0.02, + // Biquad filter for engine vibration removal (based on ArduPilot research) + lon_filter_cutoff: 5.0, // 5Hz cutoff preserves driving dynamics (0-3Hz) + lon_sample_rate: 20.0, // Matches telemetry rate }; let mut sensor_fusion = SensorFusion::new(fusion_config); @@ -750,6 +753,13 @@ fn main() { // Feed GPS speed to sensor fusion for acceleration calculation let time_s = now_ms as f32 / 1000.0; sensor_fusion.process_gps(speed, time_s); + + // Feed GPS heading to yaw rate calibrator (only valid at speed) + // Course over ground is unreliable below ~5 m/s + if speed > 5.0 { + let course = sensors.gps_parser.last_fix().course; + sensor_fusion.process_gps_heading(course); + } } } } diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index 3cfda31..070dc13 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -340,10 +340,11 @@ impl TelemetryPublisher { now_ms: u32, ) -> Result<(), SystemError> { // Get accelerations for display - // lon: GPS/IMU blended (same as mode classifier input) - // lat: accelerometer-based (mode uses centripetal = speed × yaw_rate) - let lon_blended = sensor_fusion.get_lon_blended(); - let lat_display = sensor_fusion.get_lat_display(); + // lon: GPS-derived when fresh (no vibration), otherwise blended with heavy smoothing + // lat: centripetal (speed × yaw_rate) - instant, doesn't stick after corners + // Uses calibrated yaw rate to prevent drift on highway + let lon_display = sensor_fusion.get_lon_display(); + let lat_display = sensor_fusion.get_lat_centripetal(); // Keep raw az for reference (no tilt correction needed for vertical) let (_, _, az_corr) = sensors.imu_parser.get_accel_corrected(); @@ -355,7 +356,7 @@ impl TelemetryPublisher { // Dashboard expects: lng = -ax/9.81, latg = ay/9.81 // So: ax = -lon (negated so dashboard shows positive for accel) // ay = lat (positive = right turn on G-meter) - packet.ax = -lon_blended; + packet.ax = -lon_display; packet.ay = -lat_display; // Negate: fusion uses left-positive, dashboard uses right-positive packet.az = az_corr; packet.wz = sensors.imu_parser.data().wz; From 9b9221aaee8f1f40947b222b1e017b1366b3b164 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sun, 18 Jan 2026 11:21:56 -0800 Subject: [PATCH 13/30] Fix critical filter sample rate bug and improve GPS blending Critical bug fix: - lon_sample_rate was 20Hz but filter runs at 200Hz (IMU rate) - This made filter ~10x more aggressive than intended, killing real dynamics Filter improvements: - Increase cutoff from 5Hz to 15Hz (ArduPilot uses 20Hz) - Sharp braking events (up to 10Hz) now preserved GPS blending improvements: - Reduce GPS weight from 70% to 40% (trust filtered IMU more) - Add GPS accel validity check: when GPS=0 but IMU has signal, use 100% IMU - Use lon_blended for display (was GPS-only, often showing 0) Analysis showed: - 57% of samples had lon_g=0 due to GPS-only display - Mode detection was 38% accurate for ACCEL, 43% for BRAKE - 604 samples with |GT|>0.15g but reported 0 New tests: - test_default_config_has_correct_filter_settings - test_lon_display_equals_lon_blended - test_sharp_braking_preserved_with_15hz_filter - test_gps_accel_validity_check --- CLAUDE.md | 52 ++++--- framework/src/fusion.rs | 253 ++++++++++++++++++++++++++++------- sensors/blackbox/src/main.rs | 17 ++- 3 files changed, 248 insertions(+), 74 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index ffe8499..8f290de 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -563,26 +563,25 @@ Combined states: 2nd-order Butterworth IIR filter for vibration removal from IMU longitudinal acceleration. **Problem:** -Engine vibration (20-100 Hz) causes 0.08-0.12g noise on longitudinal G readings when +Engine vibration (30-100 Hz) causes 0.08-0.12g noise on longitudinal G readings when the vehicle is running but stationary. This makes mode detection unreliable. **Solution (based on ArduPilot research):** -- ArduPilot uses 10Hz low-pass on accelerometers for outer control loops -- Academic research shows 1-5Hz Butterworth effective for vehicle dynamics -- Physics: driving events (braking, acceleration, cornering) are < 3Hz -- Engine/road vibration is 20-100Hz → easily separable from driving dynamics +- ArduPilot uses **20Hz** for INS_ACCEL_FILTER default +- Sharp braking events can have frequency components up to 5-10Hz +- Engine/road vibration is 30-100Hz → separable from driving dynamics (0-10Hz) **Configuration:** -- Sample rate: 20 Hz (telemetry rate) -- Cutoff: 5 Hz (preserves all driving dynamics, removes vibration) +- Sample rate: **200 Hz (IMU rate)** - CRITICAL: must match actual process_imu() call rate! +- Cutoff: **15 Hz** (preserves driving dynamics 0-10Hz, removes vibration 30+Hz) - Q = 0.707 (Butterworth - maximally flat passband, no overshoot) **Frequency Response:** ``` -0-3 Hz: ~100% pass (driving dynamics preserved) -5 Hz: -3 dB (cutoff point) -10 Hz: -12 dB (attenuated) -30+ Hz: -30+ dB (engine vibration removed) +0-10 Hz: ~100% pass (driving dynamics preserved) +15 Hz: -3 dB (cutoff point) +30 Hz: -12 dB (engine vibration attenuated) +60+ Hz: -24+ dB (engine vibration removed) ``` **Implementation:** @@ -623,31 +622,40 @@ Handles GPS/IMU blending, tilt correction, and continuous calibration. 5. **SensorFusion** - Main processor - Applies tilt correction - Applies gravity correction - - **5 Hz Butterworth low-pass filter** removes engine vibration (see filter.rs) + - **15 Hz Butterworth low-pass filter** removes engine vibration (see filter.rs) - Blends GPS and IMU for longitudinal acceleration - Computes centripetal lateral (speed × calibrated_yaw_rate) **GPS/IMU Blending Ratios (configurable in FusionConfig):** ``` -GPS rate >= 20Hz, fresh: 70% GPS / 30% IMU (high confidence) -GPS rate 10-20Hz: 50% GPS / 50% IMU (medium confidence) -GPS rate < 10Hz: 30% GPS / 70% IMU (low confidence) +GPS rate >= 20Hz, fresh: 40% GPS / 60% IMU (trust filtered IMU more) +GPS rate 10-20Hz: 30% GPS / 70% IMU (medium confidence) +GPS rate < 10Hz: 20% GPS / 80% IMU (low confidence) GPS stale (>200ms): 0% GPS / 100% IMU (fallback) +GPS accel ~0, IMU has signal: 0% GPS / 100% IMU (validity check) ``` -These ratios balance responsiveness (~80-100ms latency) with accuracy. -For track use, consider increasing GPS weights for maximum accuracy. +**GPS Accel Validity Check:** +When GPS-derived accel is near zero (< 0.2 m/s²) but filtered IMU shows signal (> 0.2 m/s²), +the GPS value is unreliable (just means speed didn't change between GPS samples). +In this case, GPS weight is set to 0% and 100% filtered IMU is used. + +**Why trust IMU more than GPS?** +- GPS-derived accel is `(speed_new - speed_old) / dt` which is often 0 (no speed change) +- With 15Hz Butterworth filter, IMU vibration is removed while preserving dynamics +- Responsive G readings require high IMU contribution +- GPS still provides drift correction and sanity check **Data Flow:** ``` LONGITUDINAL: GPS (25Hz) → GpsAcceleration → lon_accel_gps ────┐ │ -IMU → remove_gravity → body_to_earth → Tilt → Gravity → Biquad(5Hz) → lon_accel_imu - │ - Blend(gps, imu) ◄┘ - ↓ - lon_blended +IMU (200Hz) → remove_gravity → body_to_earth → Tilt → Gravity → Biquad(15Hz) → lon_accel_imu + │ + Blend(40% gps, 60% imu) ◄┘ + ↓ + lon_blended LATERAL (centripetal): GPS heading → YawRateCalibrator → learned_bias diff --git a/framework/src/fusion.rs b/framework/src/fusion.rs index 273e076..2a1947f 100644 --- a/framework/src/fusion.rs +++ b/framework/src/fusion.rs @@ -82,18 +82,23 @@ impl Default for FusionConfig { gps_max_age: 0.2, // 200ms // Balanced blend: 70% GPS when fresh and fast // Gives ~80-100ms latency instead of ~120-150ms - gps_weight_high: 0.70, // 70% GPS / 30% IMU at >= 20Hz - gps_weight_medium: 0.50, // 50% GPS / 50% IMU at >= 10Hz - gps_weight_low: 0.30, // 30% GPS / 70% IMU at < 10Hz + // GPS blending weights - conservative values to trust filtered IMU more + // GPS-derived accel is often 0 (no speed change between samples) + gps_weight_high: 0.40, // 40% GPS / 60% IMU at >= 20Hz + gps_weight_medium: 0.30, // 30% GPS / 70% IMU at >= 10Hz + gps_weight_low: 0.20, // 20% GPS / 80% IMU at < 10Hz tilt_learn_time: 3.0, steady_state_speed_tolerance: 0.5, // 0.5 m/s = 1.8 km/h steady_state_yaw_tolerance: 0.087, // ~5 deg/s gravity_learn_time: 2.0, gravity_alpha: 0.02, // Very slow update - // 5 Hz cutoff: passes driving dynamics (0-3Hz), removes engine vibration (20-100Hz) - // At 20 Hz sample rate, this gives ~40dB attenuation at 50Hz - lon_filter_cutoff: 5.0, - lon_sample_rate: 20.0, // Default telemetry rate + // Butterworth filter for IMU longitudinal vibration removal + // 15 Hz cutoff: passes driving dynamics (0-10Hz), removes engine vibration (30-100Hz) + // ArduPilot uses 20Hz for INS_ACCEL_FILTER; 15Hz is conservative middle ground + lon_filter_cutoff: 15.0, + // CRITICAL: Must match actual IMU sample rate, NOT telemetry rate! + // Filter is called in process_imu() which runs at IMU rate (200Hz) + lon_sample_rate: 200.0, } } } @@ -722,35 +727,39 @@ impl SensorFusion { // GPS provides smooth, drift-free acceleration from velocity changes // IMU (now filtered) provides fast response with vibration removed // Blending gives best of both: smooth when GPS available, fast filtered fallback - let gps_weight = self.compute_gps_weight(); - self.last_gps_weight = gps_weight; + let base_gps_weight = self.compute_gps_weight(); let lon_blended = if let Some(gps_lon) = self.gps_accel.get_accel() { + // GPS accel validity check: if GPS shows ~0 but IMU shows signal, + // GPS is likely just not updating (no speed change between samples) + // In this case, trust filtered IMU entirely + const GPS_ACCEL_MIN_THRESHOLD: f32 = 0.2; // m/s² (~0.02g) + + let effective_gps_weight = if gps_lon.abs() < GPS_ACCEL_MIN_THRESHOLD + && self.lon_imu_filtered.abs() > GPS_ACCEL_MIN_THRESHOLD + { + // GPS showing ~0 but IMU has signal → GPS unreliable, use 100% IMU + 0.0 + } else { + base_gps_weight + }; + + self.last_gps_weight = effective_gps_weight; + // Blend GPS with filtered IMU (vibration removed) - gps_weight * gps_lon + (1.0 - gps_weight) * self.lon_imu_filtered + effective_gps_weight * gps_lon + (1.0 - effective_gps_weight) * self.lon_imu_filtered } else { // No GPS - use filtered IMU only (still vibration-free) + self.last_gps_weight = 0.0; self.lon_imu_filtered }; self.blended_lon = lon_blended; - // Compute display-specific longitudinal - // With Butterworth filter active, blended value is already clean - // Use GPS directly when fresh for smoothest display, otherwise use blended - let lon_for_display = if let Some(gps_lon) = self.gps_accel.get_accel() { - if self.gps_accel.is_fresh(self.config.gps_max_age) { - // GPS fresh - use it directly (smoothest) - gps_lon - } else { - // GPS stale - use filtered blended (vibration already removed by Biquad) - lon_blended - } - } else { - // No GPS accel - use filtered blended - lon_blended - }; - self.lon_display = lon_for_display; + // Display uses same blended value as mode detection + // GPS-only was problematic: GPS-derived accel is often 0 (no speed change between samples) + // With 15Hz Butterworth filter, vibration is removed and IMU is responsive + self.lon_display = lon_blended; // Return blended longitudinal and centripetal lateral for mode detection (lon_blended, self.lat_centripetal) @@ -1135,17 +1144,17 @@ mod tests { #[test] fn test_biquad_filter_removes_engine_vibration() { // Simulate engine vibration at 30Hz with 0.1g amplitude (~1 m/s²) - // The 5Hz Butterworth filter should attenuate this by ~30dB + // The 15Hz Butterworth filter should attenuate this by ~12dB let config = FusionConfig::default(); let mut fusion = SensorFusion::new(config); let vibration_freq = 30.0; // Hz - typical engine vibration let vibration_amp = 1.0; // m/s² (~0.1g) - let sample_rate = 20.0; // Hz - telemetry rate + let sample_rate = 200.0; // Hz - IMU rate (must match config!) // Run for 2 seconds to let filter settle let mut max_output: f32 = 0.0; - for i in 0..40 { + for i in 0..400 { let t = i as f32 / sample_rate; // Simulate vibrating IMU input in earth frame let vibration = vibration_amp * (2.0 * core::f32::consts::PI * vibration_freq * t).sin(); @@ -1156,22 +1165,22 @@ mod tests { 0.0, // yaw = 0 (heading east) 10.0, // speed 0.0, // yaw_rate = 0 - 0.05, // dt = 50ms + 0.005, // dt = 5ms (200Hz) false, // not stationary ); - // After settling (1 second = 20 samples), check output - if i > 20 { + // After settling (1 second = 200 samples), check output + if i > 200 { max_output = max_output.max(lon.abs()); } } - // 30Hz vibration should be attenuated by ~30dB at 5Hz cutoff - // That means 1 m/s² input → ~0.03 m/s² output (or less) - // Being conservative, check it's below 0.1 m/s² (~0.01g) + // 30Hz vibration should be attenuated by ~12dB at 15Hz cutoff + // That means 1 m/s² input → ~0.25 m/s² output (or less) + // Being conservative, check it's below 0.4 m/s² (~0.04g) assert!( - max_output < 0.15, - "30Hz vibration should be heavily attenuated: got {} m/s²", + max_output < 0.4, + "30Hz vibration should be attenuated: got {} m/s²", max_output ); } @@ -1179,17 +1188,17 @@ mod tests { #[test] fn test_biquad_filter_passes_driving_dynamics() { // Simulate a braking event at 1Hz (typical vehicle dynamics) - // The 5Hz filter should pass this with minimal attenuation + // The 15Hz filter should pass this with minimal attenuation let config = FusionConfig::default(); let mut fusion = SensorFusion::new(config); - let dynamics_freq = 1.0; // Hz - braking event - let dynamics_amp = 3.0; // m/s² (~0.3g braking) - let sample_rate = 20.0; // Hz + let dynamics_freq = 1.0; // Hz - braking event + let dynamics_amp = 3.0; // m/s² (~0.3g braking) + let sample_rate = 200.0; // Hz - IMU rate (must match config!) // Run for 3 seconds let mut max_output: f32 = 0.0; - for i in 0..60 { + for i in 0..600 { let t = i as f32 / sample_rate; let signal = dynamics_amp * (2.0 * core::f32::consts::PI * dynamics_freq * t).sin(); @@ -1199,12 +1208,12 @@ mod tests { 0.0, 10.0, 0.0, - 0.05, + 0.005, // dt = 5ms (200Hz) false, ); - // After settling (1 second), check output - if i > 20 { + // After settling (1 second = 200 samples), check output + if i > 200 { max_output = max_output.max(lon.abs()); } } @@ -1217,4 +1226,158 @@ mod tests { max_output ); } + + #[test] + fn test_default_config_has_correct_filter_settings() { + // Verify critical configuration values + let config = FusionConfig::default(); + + // Filter must be configured for IMU rate (200Hz), NOT telemetry rate (20Hz) + assert_eq!( + config.lon_sample_rate, 200.0, + "lon_sample_rate must be 200Hz (IMU rate)" + ); + + // Filter cutoff should be 15Hz (ArduPilot uses 20Hz) + assert_eq!( + config.lon_filter_cutoff, 15.0, + "lon_filter_cutoff should be 15Hz" + ); + + // GPS weights should be conservative (trust filtered IMU more) + assert!( + config.gps_weight_high <= 0.5, + "GPS weight should be <=50% to trust filtered IMU" + ); + } + + #[test] + fn test_lon_display_equals_lon_blended() { + // Verify that lon_display uses lon_blended, not GPS-only + // This ensures dashboard shows same value as mode detection + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Feed GPS data + fusion.process_gps(10.0, 0.0); + fusion.process_gps(10.5, 0.1); // Creates GPS accel + + // Process IMU with non-zero input + let (lon_blended, _lat) = fusion.process_imu( + 2.0, // Earth X accel + 0.0, // Earth Y + 0.0, // Heading + 10.0, // Speed + 0.0, // Yaw rate + 0.005, // dt + false, // Not stationary + ); + + let lon_display = fusion.get_lon_display(); + + // Display should equal blended (not GPS-only) + assert!( + (lon_display - lon_blended).abs() < 0.001, + "lon_display ({}) should equal lon_blended ({})", + lon_display, + lon_blended + ); + } + + #[test] + fn test_sharp_braking_preserved_with_15hz_filter() { + // Simulate sharp braking (0.3 second event = ~3Hz component) + // 15Hz filter should preserve this; old 5Hz filter would attenuate + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + let sample_rate = 200.0; + let mut max_output: f32 = 0.0; + + // 0.3 second braking pulse + for i in 0..200 { + let t = i as f32 / sample_rate; + + // Sharp braking pulse: ramps up in 0.1s, holds 0.1s, ramps down 0.1s + let brake_input = if t < 0.1 { + 5.0 * (t / 0.1) // Ramp up to 5 m/s² (0.5g) + } else if t < 0.2 { + 5.0 // Hold + } else if t < 0.3 { + 5.0 * (1.0 - (t - 0.2) / 0.1) // Ramp down + } else { + 0.0 + }; + + let (lon, _lat) = fusion.process_imu( + brake_input, + 0.0, + 0.0, + 15.0, // 15 m/s = 54 km/h + 0.0, + 0.005, + false, + ); + + max_output = max_output.max(lon.abs()); + } + + // Should preserve at least 60% of 5 m/s² input + // (some attenuation due to filter, but not severe) + assert!( + max_output > 3.0, + "Sharp braking should be preserved: got {} m/s², expected >3.0", + max_output + ); + } + + #[test] + fn test_gps_accel_validity_check() { + // When GPS accel is ~0 but IMU shows signal, should use 100% IMU + // This prevents blending with "nothing" when GPS speed doesn't change + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Set up GPS with zero acceleration (no speed change) + fusion.process_gps(10.0, 0.0); + fusion.process_gps(10.0, 0.1); // Same speed = 0 accel + + // Process IMU with significant acceleration (braking at 0.3g) + // Need to run multiple iterations for filter to settle + let imu_accel = 3.0; // m/s² = ~0.3g + let mut lon = 0.0; + + for i in 0..100 { + let (l, _lat) = fusion.process_imu( + imu_accel, + 0.0, + 0.0, + 10.0, // speed + 0.0, // yaw_rate + 0.005, + false, + ); + // Keep GPS "stale" by not updating it, but refresh timestamp + if i % 20 == 0 { + fusion.process_gps(10.0, 0.1 + (i as f32) * 0.005); + } + lon = l; + } + + // GPS weight should have been set to 0 (validity check triggered) + // So output should be close to filtered IMU, not blended toward 0 + // With 15Hz filter settled, expect at least 50% of input + assert!( + lon > 1.5, + "When GPS=0 but IMU has signal, should use IMU: got {} m/s²", + lon + ); + + // Verify GPS weight was reduced + assert!( + fusion.last_gps_weight < 0.1, + "GPS weight should be ~0 when GPS accel invalid: got {}", + fusion.last_gps_weight + ); + } } diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 01cb5e4..3d09842 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -461,18 +461,21 @@ fn main() { gps_high_rate: 20.0, gps_medium_rate: 10.0, gps_max_age: 0.2, - // Balanced blend for city/highway/canyon - gps_weight_high: 0.70, // 70% GPS / 30% IMU at 25Hz - gps_weight_medium: 0.50, // 50% / 50% at 10-20Hz - gps_weight_low: 0.30, // 30% GPS / 70% IMU fallback + // Conservative GPS weights - GPS-derived accel often 0 (no speed change between samples) + // Trust filtered IMU more for responsive G readings + gps_weight_high: 0.40, // 40% GPS / 60% IMU at 25Hz + gps_weight_medium: 0.30, // 30% GPS / 70% IMU at 10-20Hz + gps_weight_low: 0.20, // 20% GPS / 80% IMU fallback tilt_learn_time: 3.0, gravity_learn_time: 2.0, steady_state_speed_tolerance: 0.5, steady_state_yaw_tolerance: 0.087, gravity_alpha: 0.02, - // Biquad filter for engine vibration removal (based on ArduPilot research) - lon_filter_cutoff: 5.0, // 5Hz cutoff preserves driving dynamics (0-3Hz) - lon_sample_rate: 20.0, // Matches telemetry rate + // Butterworth filter for vibration removal (ArduPilot uses 20Hz, we use 15Hz) + // 15Hz preserves driving dynamics (0-10Hz), removes engine vibration (30-100Hz) + lon_filter_cutoff: 15.0, + // CRITICAL: Must be IMU rate (200Hz), NOT telemetry rate! + lon_sample_rate: 200.0, }; let mut sensor_fusion = SensorFusion::new(fusion_config); From 40a0983fd3c7969da4af280ced7bfb0804c887ba Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sun, 18 Jan 2026 14:58:46 -0800 Subject: [PATCH 14/30] Remove GravityEstimator - was learning bad offsets while driving MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The GravityEstimator tried to learn gravity offset during "steady state" driving (constant speed, low yaw). This was fundamentally flawed because: - Aerodynamic drag, rolling resistance, and road grade create real accelerations - These were incorrectly learned as "gravity offsets" - Result: -2.6 m/s² constant offset causing 76% false BRAKE detections Changes: - Remove GravityEstimator struct and all references - Remove gravity fields from FusionDiagnostics and dashboard - Add 3 new unit tests to catch similar regressions: - test_cruising_at_constant_speed_produces_zero_lon - test_tilt_only_updates_when_stationary - test_no_drift_during_extended_cruise - Update CLAUDE.md documentation - Fix stale comment (5Hz -> 15Hz filter) The TiltEstimator (learns only when stationary) is sufficient for mounting offset correction. This matches ArduPilot's approach: only learn corrections when you KNOW the truth (stationary = zero velocity). --- CLAUDE.md | 13 +- framework/src/fusion.rs | 341 ++++++++++++++--------- framework/src/lib.rs | 3 +- sensors/blackbox/src/diagnostics.rs | 47 ++++ sensors/blackbox/src/main.rs | 28 +- sensors/blackbox/src/websocket_server.rs | 51 +++- 6 files changed, 322 insertions(+), 161 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 8f290de..3d9100e 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -607,21 +607,14 @@ Handles GPS/IMU blending, tilt correction, and continuous calibration. - Applies correction to all future readings - Relearns at every stop (adapts to device repositioning) -3. **GravityEstimator** - Learns gravity offset while driving - - Detects "steady state": constant speed (±0.5 m/s), low yaw (±5°/s) - - During steady state, expected acceleration ≈ 0 - - Slowly updates gravity estimate (α=0.02, ~50 second convergence) - - Essential for track/canyon driving where stops are rare - -4. **YawRateCalibrator** - Learns gyro bias while driving straight +3. **YawRateCalibrator** - Learns gyro bias while driving straight - When GPS heading is stable (±0.5°/s for 2+ seconds), assumes true yaw rate ≈ 0 - Any measured yaw rate during this time is gyro bias - Applies correction to prevent lateral drift in centripetal calculation - Essential for highway driving where ZUPT rarely triggers -5. **SensorFusion** - Main processor - - Applies tilt correction - - Applies gravity correction +4. **SensorFusion** - Main processor + - Applies tilt correction (learned when stopped) - **15 Hz Butterworth low-pass filter** removes engine vibration (see filter.rs) - Blends GPS and IMU for longitudinal acceleration - Computes centripetal lateral (speed × calibrated_yaw_rate) diff --git a/framework/src/fusion.rs b/framework/src/fusion.rs index 2a1947f..fc9775b 100644 --- a/framework/src/fusion.rs +++ b/framework/src/fusion.rs @@ -44,14 +44,6 @@ pub struct FusionConfig { pub gps_weight_low: f32, /// Time required at stop to learn tilt offset (seconds) pub tilt_learn_time: f32, - /// Speed threshold for steady-state detection (m/s) - pub steady_state_speed_tolerance: f32, - /// Yaw rate threshold for steady-state (rad/s) - pub steady_state_yaw_tolerance: f32, - /// Time required in steady state to update gravity estimate (seconds) - pub gravity_learn_time: f32, - /// Alpha for gravity estimate update (smaller = slower, more stable) - pub gravity_alpha: f32, /// Low-pass filter cutoff for IMU longitudinal (Hz) /// Removes engine vibration (20-100Hz) while passing driving dynamics (0-3Hz) /// Research: ArduPilot uses 10Hz, academic papers suggest 1-5Hz for vehicle dynamics @@ -63,35 +55,26 @@ pub struct FusionConfig { impl Default for FusionConfig { /// Default configuration balanced for city/highway/canyon driving. /// - /// GPS weights are moderate (70/50/30) for balanced responsiveness: - /// - Fast enough for canyon driving (~80-100ms latency) - /// - Smooth enough for city/highway (no jitter) + /// GPS weights are conservative (40/30/20) to trust filtered IMU more: + /// - GPS-derived accel is often 0 (no speed change between samples) + /// - Filtered IMU provides faster response /// - /// The 5 Hz Butterworth filter removes engine vibration (20-100Hz) while - /// preserving all driving dynamics (0-3Hz). This is based on: - /// - ArduPilot: uses 10Hz low-pass on accelerometers + /// The 15 Hz Butterworth filter removes engine vibration (30-100Hz) while + /// preserving all driving dynamics (0-10Hz). This is based on: + /// - ArduPilot: uses 20Hz for INS_ACCEL_FILTER /// - Research: shows 1-5Hz Butterworth effective for brake detection /// - Physics: driving events (braking, cornering) are all < 3Hz - /// - /// For track use, consider increasing GPS weights (90/70/50) for - /// maximum accuracy at the cost of some latency. fn default() -> Self { Self { gps_high_rate: 20.0, gps_medium_rate: 10.0, gps_max_age: 0.2, // 200ms - // Balanced blend: 70% GPS when fresh and fast - // Gives ~80-100ms latency instead of ~120-150ms // GPS blending weights - conservative values to trust filtered IMU more // GPS-derived accel is often 0 (no speed change between samples) gps_weight_high: 0.40, // 40% GPS / 60% IMU at >= 20Hz gps_weight_medium: 0.30, // 30% GPS / 70% IMU at >= 10Hz gps_weight_low: 0.20, // 20% GPS / 80% IMU at < 10Hz tilt_learn_time: 3.0, - steady_state_speed_tolerance: 0.5, // 0.5 m/s = 1.8 km/h - steady_state_yaw_tolerance: 0.087, // ~5 deg/s - gravity_learn_time: 2.0, - gravity_alpha: 0.02, // Very slow update // Butterworth filter for IMU longitudinal vibration removal // 15 Hz cutoff: passes driving dynamics (0-10Hz), removes engine vibration (30-100Hz) // ArduPilot uses 20Hz for INS_ACCEL_FILTER; 15Hz is conservative middle ground @@ -322,109 +305,10 @@ impl TiltEstimator { (ax_earth, ay_earth) } } -} - -/// Moving gravity estimator - learns gravity offset while driving -pub struct GravityEstimator { - /// Estimated gravity offset X (earth frame, m/s²) - offset_x: f32, - /// Estimated gravity offset Y (earth frame, m/s²) - offset_y: f32, - /// Is the estimate valid? - valid: bool, - /// Speed history for steady-state detection - speed_history: [f32; 10], - speed_idx: usize, - /// Time in steady state (seconds) - steady_time: f32, - /// Configuration - speed_tolerance: f32, - yaw_tolerance: f32, - learn_time: f32, - alpha: f32, -} - -impl GravityEstimator { - pub fn new(config: &FusionConfig) -> Self { - Self { - offset_x: 0.0, - offset_y: 0.0, - valid: false, - speed_history: [0.0; 10], - speed_idx: 0, - steady_time: 0.0, - speed_tolerance: config.steady_state_speed_tolerance, - yaw_tolerance: config.steady_state_yaw_tolerance, - learn_time: config.gravity_learn_time, - alpha: config.gravity_alpha, - } - } - - /// Update with current driving state - /// - /// # Arguments - /// * `ax_earth` - X acceleration (earth frame, m/s²) - /// * `ay_earth` - Y acceleration (earth frame, m/s²) - /// * `speed` - Current speed (m/s) - /// * `yaw_rate` - Current yaw rate (rad/s) - /// * `dt` - Time step (seconds) - pub fn update( - &mut self, - ax_earth: f32, - ay_earth: f32, - speed: f32, - yaw_rate: f32, - dt: f32, - ) -> bool { - // Update speed history - self.speed_history[self.speed_idx] = speed; - self.speed_idx = (self.speed_idx + 1) % self.speed_history.len(); - - // Check if in steady state (constant velocity) - let speed_stable = self.is_speed_stable(); - let yaw_low = yaw_rate.abs() < self.yaw_tolerance; - let moving = speed > 2.0; // Must be moving (not stopped) - - if speed_stable && yaw_low && moving { - self.steady_time += dt; - - if self.steady_time >= self.learn_time { - // In steady state, expected acceleration is ~0 - // Any measured acceleration is gravity/mounting error - self.offset_x = (1.0 - self.alpha) * self.offset_x + self.alpha * ax_earth; - self.offset_y = (1.0 - self.alpha) * self.offset_y + self.alpha * ay_earth; - self.valid = true; - return true; - } - } else { - self.steady_time = 0.0; - } - - false - } - fn is_speed_stable(&self) -> bool { - if self.speed_history[0] < 1.0 { - return false; // Need some speed data - } - - let mean: f32 = self.speed_history.iter().sum::() / self.speed_history.len() as f32; - let max_dev = self - .speed_history - .iter() - .map(|&s| (s - mean).abs()) - .fold(0.0f32, |a, b| a.max(b)); - - max_dev < self.speed_tolerance - } - - /// Apply gravity correction to acceleration - pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { - if self.valid { - (ax_earth - self.offset_x, ay_earth - self.offset_y) - } else { - (ax_earth, ay_earth) - } + /// Get learned offsets and validity (for diagnostics) + pub fn get_offsets(&self) -> (f32, f32, bool) { + (self.offset_x, self.offset_y, self.offset_valid) } } @@ -601,8 +485,6 @@ pub struct SensorFusion { pub gps_accel: GpsAcceleration, /// Tilt estimator (learns when stopped) pub tilt_estimator: TiltEstimator, - /// Gravity estimator (learns while driving) - pub gravity_estimator: GravityEstimator, /// Yaw rate calibrator (learns gyro bias while driving straight) pub yaw_rate_calibrator: YawRateCalibrator, /// Butterworth low-pass filter for IMU longitudinal (removes engine vibration) @@ -630,7 +512,6 @@ impl SensorFusion { Self { gps_accel: GpsAcceleration::new(), tilt_estimator: TiltEstimator::new(config.tilt_learn_time), - gravity_estimator: GravityEstimator::new(&config), yaw_rate_calibrator: YawRateCalibrator::new(), lon_filter: BiquadFilter::new_lowpass(config.lon_filter_cutoff, config.lon_sample_rate), blended_lon: 0.0, @@ -675,10 +556,9 @@ impl SensorFusion { self.gps_accel.advance_time(dt); // Apply tilt correction (learned when stopped) - let (ax_tilt, ay_tilt) = self.tilt_estimator.correct(ax_earth, ay_earth); - - // Apply gravity correction (learned while driving) - let (ax_corr, ay_corr) = self.gravity_estimator.correct(ax_tilt, ay_tilt); + // This handles mounting angle offset - the device learns the residual + // acceleration when stationary and subtracts it while moving. + let (ax_corr, ay_corr) = self.tilt_estimator.correct(ax_earth, ay_earth); // Transform to vehicle frame let (lon_imu_raw, lat_imu_raw) = earth_to_car(ax_corr, ay_corr, yaw); @@ -686,9 +566,9 @@ impl SensorFusion { // Store raw IMU longitudinal (before filtering) self.lon_imu_raw = lon_imu_raw; - // Apply Butterworth low-pass filter to remove engine vibration (20-100Hz) - // Filter cutoff is 5Hz: passes driving dynamics (0-3Hz), removes vibration - // Based on ArduPilot (10Hz on accels) and research (1-5Hz for vehicle dynamics) + // Apply Butterworth low-pass filter to remove engine vibration (30-100Hz) + // Filter cutoff is 15Hz: passes driving dynamics (0-5Hz), attenuates vibration + // Based on ArduPilot (10-20Hz on accels, INS_ACCEL_FILTER parameter) let lon_imu_filtered = self.lon_filter.process(lon_imu_raw); self.lon_imu_filtered = lon_imu_filtered; @@ -714,10 +594,6 @@ impl SensorFusion { self.tilt_estimator.reset_stationary(); } - // Update gravity estimate while driving - self.gravity_estimator - .update(ax_earth, ay_earth, speed, yaw_rate, dt); - // Update yaw rate calibrator (learns bias while driving straight) self.yaw_rate_calibrator.update(yaw_rate, speed, dt); } @@ -819,6 +695,45 @@ impl SensorFusion { self.lat_centripetal } + // ========== Diagnostic getters ========== + + /// Get raw IMU longitudinal acceleration (before filter, m/s²) + pub fn get_lon_imu_raw(&self) -> f32 { + self.lon_imu_raw + } + + /// Get filtered IMU longitudinal acceleration (after Butterworth, m/s²) + pub fn get_lon_imu_filtered(&self) -> f32 { + self.lon_imu_filtered + } + + /// Get last GPS blend weight (0.0-1.0) + pub fn get_last_gps_weight(&self) -> f32 { + self.last_gps_weight + } + + /// Get GPS-derived acceleration (m/s²), or NaN if invalid + pub fn get_gps_accel(&self) -> f32 { + self.gps_accel.get_accel().unwrap_or(f32::NAN) + } + + /// Get GPS rate estimate (Hz) + pub fn get_gps_rate(&self) -> f32 { + self.gps_accel.get_rate() + } + + /// Check if GPS was rejected by validity check + /// Returns true if GPS showed ~0 but IMU had signal + pub fn is_gps_rejected(&self) -> bool { + // If we have valid GPS but weight is 0, GPS was rejected + self.gps_accel.get_accel().is_some() && self.last_gps_weight < 0.01 + } + + /// Get tilt estimator offsets and validity + pub fn get_tilt_offsets(&self) -> (f32, f32, bool) { + self.tilt_estimator.get_offsets() + } + /// Compute GPS weight based on GPS rate and data freshness fn compute_gps_weight(&self) -> f32 { let rate = self.gps_accel.get_rate(); @@ -1380,4 +1295,152 @@ mod tests { fusion.last_gps_weight ); } + + #[test] + fn test_cruising_at_constant_speed_produces_zero_lon() { + // KEY BUG TEST: When cruising at constant speed with no real acceleration, + // lon_blended should be near zero. This catches bad offset learning. + // + // The GravityEstimator bug caused lon_raw = -2.6 m/s² when cruising, + // which triggered false BRAKE detection. + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // First: learn tilt offset when stopped (simulates calibration) + for _ in 0..600 { + // 3 seconds at 200Hz + fusion.process_imu( + 0.0, // No earth X accel (level surface) + 0.0, // No earth Y accel + 0.0, // Heading + 0.0, // Stopped + 0.0, // No yaw rate + 0.005, // dt = 5ms + true, // STATIONARY - tilt learns here + ); + } + + // Now: start "cruising" at 20 m/s with zero input acceleration + // This simulates constant-speed highway driving + let mut max_lon: f32 = 0.0; + let mut sum_lon: f32 = 0.0; + let samples = 400; // 2 seconds at 200Hz + + for i in 0..samples { + // Provide GPS updates to keep it fresh + if i % 8 == 0 { + // 25Hz GPS rate + fusion.process_gps(20.0, (i as f32) * 0.005); // Constant speed + } + + let (lon, _lat) = fusion.process_imu( + 0.0, // ZERO earth X accel - no acceleration! + 0.0, // Zero earth Y accel + 0.0, // Heading + 20.0, // 20 m/s = 72 km/h cruising + 0.0, // Straight road + 0.005, // dt = 5ms + false, // Moving + ); + + // Skip filter settling time + if i > 200 { + max_lon = max_lon.max(lon.abs()); + sum_lon += lon.abs(); + } + } + + let avg_lon = sum_lon / (samples - 200) as f32; + + // CRITICAL: lon_blended should be near zero when cruising + // The bug caused ~2.6 m/s² here + assert!( + max_lon < 0.5, + "Cruising with zero input should produce near-zero lon: max={} m/s²", + max_lon + ); + assert!( + avg_lon < 0.2, + "Average lon during cruise should be near zero: avg={} m/s²", + avg_lon + ); + } + + #[test] + fn test_tilt_only_updates_when_stationary() { + // Verify that tilt estimator only learns when is_stationary=true + // If it learned while moving, bad offsets could accumulate + let mut tilt = TiltEstimator::new(1.0); + + // Simulate "moving" conditions with large acceleration + for _ in 0..300 { + // 1.5 seconds at 200Hz + // Note: update_stationary should only be called when stationary, + // but let's verify the valid flag behavior + tilt.update_stationary(5.0, 3.0, 0.005); // Large accel (would be wrong if learned) + } + + // After learning, check the offset + let (ax, _ay) = tilt.correct(5.0, 3.0); + + // The tilt estimator WILL learn this offset - this is by design + // The key is that in main.rs, we only call update_stationary when truly stopped + assert!( + ax.abs() < 0.1, + "After learning, correction should work: {}", + ax + ); + } + + #[test] + fn test_no_drift_during_extended_cruise() { + // Verify that lon_blended stays near zero over extended cruising + // This catches slow drift from any remaining estimators + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Learn tilt when stopped + for _ in 0..600 { + fusion.process_imu(0.0, 0.0, 0.0, 0.0, 0.0, 0.005, true); + } + + // Start cruising + let mut lon_samples: Vec = Vec::new(); + + for i in 0..2000 { + // 10 seconds at 200Hz + if i % 8 == 0 { + fusion.process_gps(25.0, (i as f32) * 0.005); + } + + let (lon, _) = fusion.process_imu( + 0.0, // Zero input + 0.0, + 0.0, + 25.0, // 25 m/s = 90 km/h + 0.0, + 0.005, + false, + ); + + if i > 400 { + // After 2 seconds settling + lon_samples.push(lon); + } + } + + // Check for drift: first half vs second half average + let mid = lon_samples.len() / 2; + let first_half_avg: f32 = lon_samples[..mid].iter().sum::() / mid as f32; + let second_half_avg: f32 = lon_samples[mid..].iter().sum::() / mid as f32; + let drift = (second_half_avg - first_half_avg).abs(); + + assert!( + drift < 0.1, + "lon should not drift during cruise: first_avg={}, second_avg={}, drift={}", + first_half_avg, + second_half_avg, + drift + ); + } } diff --git a/framework/src/lib.rs b/framework/src/lib.rs index 0c6a7ff..7dcddc4 100644 --- a/framework/src/lib.rs +++ b/framework/src/lib.rs @@ -145,8 +145,7 @@ pub use mode::{Mode, ModeClassifier, ModeConfig, DEFAULT_MODE_ALPHA}; // Re-export sensor fusion types pub use fusion::{ - FusionConfig, GpsAcceleration, GravityEstimator, SensorFusion, TiltEstimator, - YawRateCalibrator, + FusionConfig, GpsAcceleration, SensorFusion, TiltEstimator, YawRateCalibrator, }; // Re-export commonly used types from sensor_framework diff --git a/sensors/blackbox/src/diagnostics.rs b/sensors/blackbox/src/diagnostics.rs index 747193d..ab957ff 100644 --- a/sensors/blackbox/src/diagnostics.rs +++ b/sensors/blackbox/src/diagnostics.rs @@ -94,6 +94,42 @@ pub struct ConfigSnapshot { pub gps_warmup_fixes: u8, } +/// Sensor fusion diagnostics - filter pipeline and blending status +#[derive(Debug, Clone, Copy, Default)] +pub struct FusionDiagnostics { + // Filter pipeline (all in m/s²) + /// Raw IMU longitudinal acceleration (before Butterworth filter) + pub lon_imu_raw: f32, + /// Filtered IMU longitudinal (after 15Hz low-pass) + pub lon_imu_filtered: f32, + /// Final blended longitudinal (GPS/IMU mix) + pub lon_blended: f32, + + // GPS blending status + /// Current GPS blend weight (0.0-0.4 typically) + pub gps_weight: f32, + /// GPS-derived acceleration (m/s²), NaN if invalid + pub gps_accel: f32, + /// GPS update rate estimate (Hz) + pub gps_rate: f32, + /// Was GPS rejected by validity check? (GPS=0 but IMU has signal) + pub gps_rejected: bool, + + // Yaw rate calibrator + /// Learned gyro bias (rad/s) + pub yaw_bias: f32, + /// Is yaw calibration valid? + pub yaw_calibrated: bool, + + // Tilt estimator (learned when stopped) + /// Tilt offset X (m/s²) + pub tilt_offset_x: f32, + /// Tilt offset Y (m/s²) + pub tilt_offset_y: f32, + /// Is tilt offset valid? + pub tilt_valid: bool, +} + /// Complete diagnostics snapshot (immutable copy for reading) #[derive(Debug, Clone, Default)] pub struct DiagnosticsSnapshot { @@ -103,6 +139,7 @@ pub struct DiagnosticsSnapshot { pub system_health: SystemHealth, pub wifi_status: WifiStatus, pub config: ConfigSnapshot, + pub fusion: FusionDiagnostics, } /// Thread-safe diagnostics state container @@ -128,6 +165,7 @@ struct DiagnosticsInner { system_health: SystemHealth, wifi_status: WifiStatus, config: ConfigSnapshot, + fusion: FusionDiagnostics, // For rate calculation (last known counts) last_imu_count: u32, last_gps_count: u32, @@ -300,6 +338,14 @@ impl DiagnosticsState { } } + /// Update fusion diagnostics (filter pipeline, GPS blending, calibrators) + #[allow(clippy::too_many_arguments)] + pub fn update_fusion(&self, fusion: FusionDiagnostics) { + if let Ok(mut inner) = self.inner.lock() { + inner.fusion = fusion; + } + } + /// Get a snapshot of all diagnostics (for HTTP response) pub fn snapshot(&self) -> DiagnosticsSnapshot { if let Ok(inner) = self.inner.lock() { @@ -310,6 +356,7 @@ impl DiagnosticsState { system_health: inner.system_health, wifi_status: inner.wifi_status.clone(), config: inner.config.clone(), + fusion: inner.fusion, } } else { DiagnosticsSnapshot::default() diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 3d09842..721a494 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -17,7 +17,7 @@ mod wifi; use std::sync::Arc; use config::{SystemConfig, WifiModeConfig}; -use diagnostics::DiagnosticsState; +use diagnostics::{DiagnosticsState, FusionDiagnostics}; use fusion::{FusionConfig, SensorFusion}; use esp_idf_hal::{ delay::FreeRtos, @@ -450,9 +450,8 @@ fn main() { // // This provides: // 1. Tilt correction - learns mounting offset when stopped (3s) - // 2. Gravity correction - learns while driving at steady speed - // 3. GPS/IMU blending - 70% GPS at 25Hz for smooth, drift-free longitudinal - // 4. Centripetal lateral - speed × yaw_rate for instant corner detection + // 2. GPS/IMU blending - 40% GPS at 25Hz for smooth, drift-free longitudinal + // 3. Centripetal lateral - speed × yaw_rate for instant corner detection // // Latency: ~100-150ms for ACCEL/BRAKE (GPS blend + EMA) // ~0ms for CORNER (centripetal is instant) @@ -467,10 +466,6 @@ fn main() { gps_weight_medium: 0.30, // 30% GPS / 70% IMU at 10-20Hz gps_weight_low: 0.20, // 20% GPS / 80% IMU fallback tilt_learn_time: 3.0, - gravity_learn_time: 2.0, - steady_state_speed_tolerance: 0.5, - steady_state_yaw_tolerance: 0.087, - gravity_alpha: 0.02, // Butterworth filter for vibration removal (ArduPilot uses 20Hz, we use 15Hz) // 15Hz preserves driving dynamics (0-10Hz), removes engine vibration (30-100Hz) lon_filter_cutoff: 15.0, @@ -583,6 +578,23 @@ fn main() { gps_fix.pdop, ); + // Fusion diagnostics (filter pipeline, GPS blending, calibrators) + let (tilt_x, tilt_y, tilt_valid) = sensor_fusion.get_tilt_offsets(); + diagnostics.update_fusion(FusionDiagnostics { + lon_imu_raw: sensor_fusion.get_lon_imu_raw(), + lon_imu_filtered: sensor_fusion.get_lon_imu_filtered(), + lon_blended: sensor_fusion.get_lon_blended(), + gps_weight: sensor_fusion.get_last_gps_weight(), + gps_accel: sensor_fusion.get_gps_accel(), + gps_rate: sensor_fusion.get_gps_rate(), + gps_rejected: sensor_fusion.is_gps_rejected(), + yaw_bias: sensor_fusion.yaw_rate_calibrator.get_bias(), + yaw_calibrated: sensor_fusion.yaw_rate_calibrator.is_valid(), + tilt_offset_x: tilt_x, + tilt_offset_y: tilt_y, + tilt_valid, + }); + publisher.reset_stats(); loop_count = 0; last_heap_check_ms = now_ms; diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 1e94344..0e70f53 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -660,6 +660,25 @@ h1{font-size:18px;margin-bottom:16px;color:#60a5fa;letter-spacing:2px}
TX Failed--
+
+

Sensor Fusion

+
+
+
Lon Raw--
+
Lon Filtered--
+
Lon Blended--
+
GPS Weight--
+
GPS Accel--
+
GPS Rejected--
+
+
+
Yaw Bias--
+
Yaw Calibrated--
+
Tilt X/Y--
+
Tilt Valid--
+
+
+
Uptime: --
+ @@ -840,7 +882,7 @@
About Features - Dashboard + How It Works Hardware GitHub Flash Firmware @@ -880,19 +922,19 @@

GPS + IMU sensor fusion for vehicle dynamics

-
+
20Hz
-
Telemetry
+
Telemetry Rate
-
+
200Hz
IMU Sampling
-
+
7-State
Kalman Filter
-
+
$50-100
Hardware Cost
@@ -1005,7 +1047,7 @@

Technical Features

Sensor Fusion

-

7-state EKF fuses 200Hz IMU with GPS. ArduPilot-style orientation correction learns and compensates for AHRS errors.

+

7-state Extended Kalman Filter combines fast IMU (200Hz) with accurate GPS. Orientation correction automatically learns mounting angle errors.

@@ -1059,8 +1101,8 @@

Mobile Dashboard

-

ZUPT

-

Zero-velocity updates prevent drift. Bias continuously estimated when stationary.

+

Zero-Velocity Updates

+

When stopped, the system knows velocity is exactly zero. This prevents position drift that would otherwise accumulate over time.

@@ -1072,8 +1114,8 @@

ZUPT

-

Orientation Correction

-

GPS velocity provides ground truth to learn and correct AHRS pitch/roll errors during acceleration. Mount anywhere.

+

Auto Mounting Correction

+

Device can be mounted at any angle (within 15°). GPS provides ground truth to automatically learn and correct orientation errors.

@@ -1091,6 +1133,84 @@

Open Source

+
+
+
+

How It Works

+

Understanding the 7-state Extended Kalman Filter and sensor fusion

+
+ + +
+

The 7 States Explained

+

+ The Extended Kalman Filter (EKF) tracks 7 values that describe your vehicle's motion. These are continuously estimated and refined as new sensor data arrives: +

+
+
+
x, y
+
Position
+
Location in meters relative to where GPS first locked. X is east-west, Y is north-south.
+
+
+
ψ (psi)
+
Yaw / Heading
+
Which direction the vehicle is pointing, in radians. 0 = East, π/2 = North.
+
+
+
vx, vy
+
Velocity
+
Speed broken into east-west and north-south components, in meters per second.
+
+
+
bax, bay
+
Accelerometer Bias
+
Sensor error offsets that slowly drift. Learned when stationary and subtracted from readings.
+
+
+
+ + +
+

Glossary of Terms

+
+
+
EKF
+
Extended Kalman Filter — A mathematical algorithm that combines multiple sensor measurements to estimate state (position, velocity, etc.) more accurately than any single sensor alone.
+
+
+
IMU
+
Inertial Measurement Unit — A sensor that measures acceleration (how fast you're speeding up/slowing down) and rotation (how fast you're turning). The WT901 used here samples at 200 times per second.
+
+
+
GPS
+
Global Positioning System — Satellite-based positioning. Provides accurate position and velocity but only updates 5-25 times per second. The EKF fills the gaps using IMU data.
+
+
+
AHRS
+
Attitude and Heading Reference System — The WT901's built-in processor that estimates orientation (pitch, roll, yaw) by combining accelerometer, gyroscope, and magnetometer data.
+
+
+
ZUPT
+
Zero-Velocity Update — When the vehicle is stopped, we know velocity is exactly zero. This constraint prevents position errors from accumulating (drifting) over time.
+
+
+
Sensor Fusion
+
Combining data from multiple sensors (IMU + GPS) to get a better estimate than either sensor alone. The IMU is fast but drifts; GPS is accurate but slow. Together they give fast AND accurate data.
+
+
+
G-Force
+
Acceleration measured in multiples of gravity (9.8 m/s²). 1g = gravity. Hard braking might be 0.8-1.0g. Cornering in a sports car might reach 1.2g.
+
+
+
WiFi AP
+
WiFi Access Point — The ESP32 creates its own wireless network that your phone connects to directly. No router or internet required.
+
+
+
+
+
+
From 8bca61e21cadcb01793deb363e3ff86592ca664d Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sun, 18 Jan 2026 17:49:57 -0800 Subject: [PATCH 17/30] Fix author name in JSON-LD structured data --- docs/index.html | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/index.html b/docs/index.html index f8e3b1f..c6c46d5 100644 --- a/docs/index.html +++ b/docs/index.html @@ -35,7 +35,7 @@ "softwareVersion": "0.1.0", "author": { "@type": "Person", - "name": "Jose Toledo" + "name": "Jose Cruz Toledo" }, "license": "https://opensource.org/licenses/MIT", "programmingLanguage": "Rust", From ed96e9e2ce48d06517f70c1ede1aab07fa0c9024 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sun, 18 Jan 2026 17:52:35 -0800 Subject: [PATCH 18/30] Add analyze_telemetry.py tool and document new features - Add analyze_telemetry.py: analyzes CSV exports from dashboard - Computes timing, speed, acceleration statistics - Compares lon_g with GPS-derived acceleration (ground truth) - Calculates mode detection precision/recall - Reports correlation coefficient and bias detection - Provides diagnostic summary of potential issues - Document drive_sim example in build commands - Document analyze_telemetry.py in Python tools section --- README.md | 20 +++ tools/python/analyze_telemetry.py | 258 ++++++++++++++++++++++++++++++ 2 files changed, 278 insertions(+) create mode 100644 tools/python/analyze_telemetry.py diff --git a/README.md b/README.md index 433a2cc..27dc44e 100644 --- a/README.md +++ b/README.md @@ -314,6 +314,9 @@ cargo clippy -- -D warnings # Run unit tests (on host, no device needed) cargo test -p sensor-fusion -p wt901 -p ublox-gps +# Run driving simulation (demonstrates GPS-corrected orientation) +cargo run -p sensor-fusion --example drive_sim + # Clean build (if things go wrong) cargo clean ``` @@ -900,6 +903,23 @@ python3 probe_wt901.py /dev/ttyUSB0 ``` Useful for verifying IMU is working before flashing, or determining what baud rate it's configured to. +**Analyze Telemetry CSV (works with both modes):** +```bash +# Export CSV from dashboard, then analyze on your computer +python3 analyze_telemetry.py recorded_session.csv +``` +Analyzes a CSV export from the dashboard and provides: +- **Timing analysis**: Sample rate, duration +- **Speed distribution**: Time in speed bands +- **Acceleration analysis**: lon_g range, mean, bias detection +- **Mode distribution**: Time spent in IDLE/ACCEL/BRAKE/CORNER +- **Ground truth comparison**: Compares lon_g with GPS-derived acceleration +- **Mode detection accuracy**: Precision and recall for ACCEL/BRAKE modes +- **Correlation**: How well lon_g matches actual acceleration (want >0.7) +- **Diagnostic summary**: Identifies potential issues like bias or high false positive rates + +This is useful for validating that the orientation correction is working properly after a test drive. + ### Choosing a Mode | Feature | Access Point | Station | diff --git a/tools/python/analyze_telemetry.py b/tools/python/analyze_telemetry.py new file mode 100644 index 0000000..32f4066 --- /dev/null +++ b/tools/python/analyze_telemetry.py @@ -0,0 +1,258 @@ +#!/usr/bin/env python3 +""" +Analyze telemetry CSV files from blackbox device. + +Usage: python analyze_telemetry.py + +CSV format expected: +time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid +""" + +import csv +import sys +from collections import Counter +import statistics + +def analyze_telemetry(filename): + with open(filename, 'r') as f: + reader = csv.DictReader(f) + rows = list(reader) + + if not rows: + print("No data in file") + return + + print(f"{'='*60}") + print(f"TELEMETRY ANALYSIS: {filename}") + print(f"{'='*60}") + print(f"Total samples: {len(rows)}") + print() + + # === TIMING ANALYSIS === + print("--- TIMING ---") + times = [int(r['time']) for r in rows] + intervals = [times[i] - times[i-1] for i in range(1, len(times))] + if intervals: + avg_interval = statistics.mean(intervals) + std_interval = statistics.stdev(intervals) if len(intervals) > 1 else 0 + print(f"Sample rate: {1000/avg_interval:.1f} Hz (interval: {avg_interval:.0f}ms +/- {std_interval:.0f}ms)") + print(f"Duration: {(times[-1] - times[0])/1000:.1f} seconds") + print() + + # === SPEED ANALYSIS === + print("--- SPEED ---") + speeds = [float(r['speed']) for r in rows] + print(f"Range: {min(speeds):.1f} to {max(speeds):.1f} km/h") + print(f"Mean: {statistics.mean(speeds):.1f} km/h") + + # Time spent in speed bands + stopped = sum(1 for s in speeds if s < 2) + slow = sum(1 for s in speeds if 2 <= s < 20) + medium = sum(1 for s in speeds if 20 <= s < 50) + fast = sum(1 for s in speeds if s >= 50) + print(f"Time distribution: stopped(<2)={100*stopped/len(rows):.0f}%, " + f"slow(2-20)={100*slow/len(rows):.0f}%, " + f"medium(20-50)={100*medium/len(rows):.0f}%, " + f"fast(>50)={100*fast/len(rows):.0f}%") + print() + + # === ACCELERATION ANALYSIS === + print("--- LONGITUDINAL ACCELERATION (lon_g in G) ---") + lon_g_vals = [float(r['lon_g']) for r in rows] + print(f"Range: {min(lon_g_vals):.3f}g to {max(lon_g_vals):.3f}g") + print(f"Mean: {statistics.mean(lon_g_vals):.4f}g (should be ~0 for balanced driving)") + print(f"Std dev: {statistics.stdev(lon_g_vals):.4f}g") + + # Distribution around zero + near_zero = sum(1 for v in lon_g_vals if abs(v) < 0.05) + positive = sum(1 for v in lon_g_vals if v >= 0.05) + negative = sum(1 for v in lon_g_vals if v <= -0.05) + print(f"Distribution: negative={100*negative/len(rows):.0f}%, " + f"near-zero={100*near_zero/len(rows):.0f}%, " + f"positive={100*positive/len(rows):.0f}%") + + # Check for bias - compare when speed is stable + stable_lon = [] + for i in range(1, len(rows)): + speed_diff = abs(float(rows[i]['speed']) - float(rows[i-1]['speed'])) + if speed_diff < 0.3: # Speed stable within 0.3 km/h + stable_lon.append(float(rows[i]['lon_g'])) + if stable_lon: + print(f"lon_g when speed stable: mean={statistics.mean(stable_lon):.4f}g " + f"(bias indicator - should be ~0)") + print() + + # === RAW ACCELEROMETER === + print("--- RAW ACCELEROMETER (ax in m/s²) ---") + ax_vals = [float(r['ax']) for r in rows] + print(f"Range: {min(ax_vals):.2f} to {max(ax_vals):.2f} m/s²") + print(f"Mean: {statistics.mean(ax_vals):.3f} m/s²") + print() + + # === MODE DISTRIBUTION === + print("--- MODE DISTRIBUTION ---") + modes = Counter(int(float(r['mode'])) for r in rows) + mode_names = {0: 'IDLE', 1: 'ACCEL', 2: 'BRAKE', 4: 'CORNER', + 5: 'ACCEL+CORNER', 6: 'BRAKE+CORNER'} + for mode, count in sorted(modes.items()): + name = mode_names.get(mode, f'UNKNOWN({mode})') + print(f" {name:15} {count:5} ({100*count/len(rows):5.1f}%)") + + total_accel = modes.get(1, 0) + modes.get(5, 0) + total_brake = modes.get(2, 0) + modes.get(6, 0) + print(f"\n Total ACCEL modes: {total_accel} ({100*total_accel/len(rows):.1f}%)") + print(f" Total BRAKE modes: {total_brake} ({100*total_brake/len(rows):.1f}%)") + print(f" Ratio ACCEL/BRAKE: {total_accel/max(total_brake,1):.1f}x (should be ~1x for balanced driving)") + print() + + # === GROUND TRUTH FROM SPEED CHANGES === + print("--- GROUND TRUTH (from speed changes) ---") + + # Calculate acceleration from speed changes + true_accels = [] + for i in range(1, len(rows)): + dt = (int(rows[i]['time']) - int(rows[i-1]['time'])) / 1000.0 # seconds + if dt > 0: + dv = (float(rows[i]['speed']) - float(rows[i-1]['speed'])) / 3.6 # m/s + accel_g = (dv / dt) / 9.80665 # in G + true_accels.append(accel_g) + + if true_accels: + print(f"GPS-derived accel range: {min(true_accels):.3f}g to {max(true_accels):.3f}g") + + # Count actual accel/brake events using speed-derived acceleration + accel_events = sum(1 for a in true_accels if a > 0.05) # > 0.05g + brake_events = sum(1 for a in true_accels if a < -0.05) # < -0.05g + coast_events = len(true_accels) - accel_events - brake_events + print(f"True events: accel={accel_events}, brake={brake_events}, coast={coast_events}") + print() + + # === MODE ACCURACY ANALYSIS === + print("--- MODE DETECTION ACCURACY ---") + + # Use lon_g thresholds (City defaults: 0.10g accel, 0.18g brake) + ACCEL_THRESH = 0.10 + BRAKE_THRESH = 0.18 + + tp_accel = fp_accel = fn_accel = 0 + tp_brake = fp_brake = fn_brake = 0 + + for i in range(1, len(rows)): + mode = int(float(rows[i]['mode'])) + lon_g = float(rows[i]['lon_g']) + + # Use speed-derived acceleration as ground truth + dt = (int(rows[i]['time']) - int(rows[i-1]['time'])) / 1000.0 + if dt > 0: + dv = (float(rows[i]['speed']) - float(rows[i-1]['speed'])) / 3.6 + true_accel_g = (dv / dt) / 9.80665 + else: + true_accel_g = 0 + + is_accel_mode = mode in [1, 5] + is_brake_mode = mode in [2, 6] + truly_accelerating = true_accel_g > 0.05 + truly_braking = true_accel_g < -0.05 + + # ACCEL accuracy + if is_accel_mode and truly_accelerating: + tp_accel += 1 + elif is_accel_mode and not truly_accelerating: + fp_accel += 1 + elif not is_accel_mode and truly_accelerating: + fn_accel += 1 + + # BRAKE accuracy + if is_brake_mode and truly_braking: + tp_brake += 1 + elif is_brake_mode and not truly_braking: + fp_brake += 1 + elif not is_brake_mode and truly_braking: + fn_brake += 1 + + print(f"ACCEL detection:") + print(f" True Positives: {tp_accel:5}") + print(f" False Positives: {fp_accel:5} (mode=ACCEL but not actually accelerating)") + print(f" False Negatives: {fn_accel:5} (accelerating but mode!=ACCEL)") + if tp_accel + fp_accel > 0: + precision = tp_accel / (tp_accel + fp_accel) + print(f" Precision: {100*precision:.1f}% (want >80%)") + if tp_accel + fn_accel > 0: + recall = tp_accel / (tp_accel + fn_accel) + print(f" Recall: {100*recall:.1f}%") + + print(f"\nBRAKE detection:") + print(f" True Positives: {tp_brake:5}") + print(f" False Positives: {fp_brake:5} (mode=BRAKE but not actually braking)") + print(f" False Negatives: {fn_brake:5} (braking but mode!=BRAKE)") + if tp_brake + fp_brake > 0: + precision = tp_brake / (tp_brake + fp_brake) + print(f" Precision: {100*precision:.1f}% (want >80%)") + if tp_brake + fn_brake > 0: + recall = tp_brake / (tp_brake + fn_brake) + print(f" Recall: {100*recall:.1f}%") + print() + + # === lon_g vs TRUE ACCEL CORRELATION === + print("--- lon_g vs GROUND TRUTH CORRELATION ---") + if len(true_accels) >= 10: + # Compare lon_g with speed-derived acceleration + lon_g_shifted = lon_g_vals[1:] # Align with true_accels + if len(lon_g_shifted) == len(true_accels): + # Calculate correlation + mean_lon = statistics.mean(lon_g_shifted) + mean_true = statistics.mean(true_accels) + + numerator = sum((l - mean_lon) * (t - mean_true) + for l, t in zip(lon_g_shifted, true_accels)) + denom_lon = sum((l - mean_lon)**2 for l in lon_g_shifted) ** 0.5 + denom_true = sum((t - mean_true)**2 for t in true_accels) ** 0.5 + + if denom_lon > 0 and denom_true > 0: + correlation = numerator / (denom_lon * denom_true) + print(f"Correlation coefficient: {correlation:.3f} (want >0.7)") + + # Calculate error + errors = [l - t for l, t in zip(lon_g_shifted, true_accels)] + mean_error = statistics.mean(errors) + rmse = (sum(e**2 for e in errors) / len(errors)) ** 0.5 + print(f"Mean error (lon_g - true): {mean_error:.4f}g (bias)") + print(f"RMSE: {rmse:.4f}g") + print() + + # === DIAGNOSTIC SUMMARY === + print("--- DIAGNOSTIC SUMMARY ---") + issues = [] + + # Check for lon_g bias + if stable_lon: + bias = statistics.mean(stable_lon) + if abs(bias) > 0.03: + issues.append(f"lon_g bias detected: {bias:.3f}g (expect ~0)") + + # Check for ACCEL/BRAKE imbalance + if total_brake > 0 and total_accel / total_brake > 3: + issues.append(f"ACCEL/BRAKE ratio too high: {total_accel/total_brake:.1f}x (suggests positive bias)") + if total_accel > 0 and total_brake / total_accel > 3: + issues.append(f"BRAKE/ACCEL ratio too high: {total_brake/total_accel:.1f}x (suggests negative bias)") + + # Check false positive rates + if tp_accel + fp_accel > 0 and fp_accel / (tp_accel + fp_accel) > 0.3: + issues.append(f"High ACCEL false positive rate: {100*fp_accel/(tp_accel+fp_accel):.0f}%") + if tp_brake + fp_brake > 0 and fp_brake / (tp_brake + fp_brake) > 0.3: + issues.append(f"High BRAKE false positive rate: {100*fp_brake/(tp_brake+fp_brake):.0f}%") + + if issues: + print("ISSUES DETECTED:") + for issue in issues: + print(f" - {issue}") + else: + print("No major issues detected") + + print(f"\n{'='*60}") + +if __name__ == '__main__': + if len(sys.argv) < 2: + print("Usage: python analyze_telemetry.py ") + sys.exit(1) + analyze_telemetry(sys.argv[1]) From f05859f9678f9dc10d93e4f4a0a09d6a4113f674 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sun, 18 Jan 2026 18:01:40 -0800 Subject: [PATCH 19/30] Improve README readability with TOC and restructured intro - Move plain English project description to the very top (before TOC) - Add table of contents with 30+ links for easy navigation - Add dedicated 'How It Works' section explaining sensor fusion in plain terms - Consolidate 'Why build this' into intro section - Document transforms, AHRS correction, and motion models for non-experts --- README.md | 95 ++++++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 76 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 27dc44e..235dfc2 100644 --- a/README.md +++ b/README.md @@ -4,13 +4,7 @@ [![Rust](https://img.shields.io/badge/rust-1.75%2B-orange.svg)](https://www.rust-lang.org/) [![ESP32-C3](https://img.shields.io/badge/platform-ESP32--C3-blue.svg)](https://www.espressif.com/) -**Real-time vehicle dynamics tracking using GPS+IMU sensor fusion.** Professional-grade algorithms on $50 hardware. - ---- - -## What Is This? - -A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's position, velocity, and acceleration in real-time. Built for track day logging, vehicle dynamics research, and DIY automotive projects. +A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's position, velocity, and acceleration in real-time. Built for track day logging, vehicle dynamics research, and DIY automotive projects. Professional-grade algorithms on $50 hardware. **What it does:** - Tracks position (±1m accuracy between GPS updates) @@ -20,18 +14,7 @@ A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's - Streams telemetry at 20-30Hz over WiFi - **Built-in mobile dashboard** - view live data on your phone -**How it works:** -- 7-state Extended Kalman Filter fuses GPS (5-25Hz) and IMU (200Hz) -- Gyro integrates yaw between GPS updates to maintain heading -- Body-frame accelerations transformed to earth frame using current orientation -- Zero-velocity updates eliminate IMU drift when stationary -- Constant Turn Rate and Acceleration (CTRA) model for cornering -- **GPS-corrected orientation** (ArduPilot-style): learns and corrects AHRS pitch/roll errors by comparing IMU with GPS velocity ground truth -- **GPS/IMU blending** for accurate, low-latency mode detection independent of mounting angle -- **Automatic tilt correction** learns device mounting offset when stopped -- **Centripetal lateral detection** uses speed × yaw_rate for instant corner response - -**Why it's useful:** +**Why build this instead of buying?** - Track day data logging without $1000+ commercial systems - Vehicle dynamics research and education - DIY EV/kit car development @@ -40,6 +23,41 @@ A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's --- +## Table of Contents + +- [Hardware Requirements](#hardware-requirements) +- [Quick Start](#quick-start) + - [Option A: Web Flasher](#option-a-web-flasher-easiest) + - [Option B: Build from Source](#option-b-build-from-source) +- [Building From Source](#building-from-source) + - [Prerequisites](#prerequisites) + - [Build Commands](#build-commands) + - [Common Build Issues](#common-build-issues) +- [System Architecture](#system-architecture) + - [Data Flow](#data-flow) + - [File Structure](#file-structure) + - [Key Algorithms](#key-algorithms) +- [Telemetry Format](#telemetry-format) +- [Configuration](#configuration) + - [WiFi and Network](#wifi-and-network) + - [EKF Tuning](#ekf-tuning) + - [EKF Health Metrics](#ekf-health-metrics) + - [Mode Detection Thresholds](#mode-detection-thresholds) +- [Performance](#performance) +- [LED Status Codes](#led-status-codes) +- [Calibration](#calibration) +- [Mobile Dashboard](#mobile-dashboard) + - [Dashboard Features](#dashboard-features) + - [Diagnostics Page](#diagnostics-page) + - [WiFi Modes](#wifi-modes) + - [Using Python Tools](#using-python-tools-station-mode-only) +- [How It Works](#how-it-works) +- [Future Enhancements](#future-enhancements) +- [Contributing](#contributing) +- [License](#license) + +--- + ## Hardware Requirements ### Core Components ($50-100 total) @@ -933,6 +951,45 @@ This is useful for validating that the orientation correction is working properl --- +## How It Works + +The system uses sensor fusion to combine fast but drifty IMU measurements with slow but accurate GPS updates. Here's what happens under the hood: + +**7-State Extended Kalman Filter** +- Tracks: position (x, y), heading (yaw), velocity (vx, vy), and accelerometer biases (bax, bay) +- Predicts state 200 times per second using IMU accelerations +- Corrects drift 5-25 times per second using GPS position and velocity +- Learns accelerometer biases during stops (Zero-Velocity Update / ZUPT) + +**Coordinate Transformations** +- Raw IMU measures in "body frame" (sensor orientation) +- Gravity is removed using roll/pitch angles +- Accelerations rotated to "earth frame" (horizontal plane) +- Then rotated to "vehicle frame" (car forward/left) for mode detection + +**GPS-Corrected Orientation (ArduPilot-style)** +- Problem: AHRS can't distinguish tilt from acceleration (reports false pitch when accelerating) +- Solution: Compare IMU-derived acceleration with GPS velocity ground truth +- Learn and correct pitch/roll errors over time +- Enables accurate 200Hz acceleration despite AHRS limitations + +**Tilt Correction** +- When stopped, the device learns its mounting offset automatically +- Works regardless of mounting angle (within reason) +- Re-learns if you move the device + +**Motion Models** +- CTRA (Constant Turn Rate and Acceleration): Used when speed >2 m/s and turning +- CA (Constant Acceleration): Used when going straight or slow + +**Why this matters for track days:** +- High-rate IMU gives responsive g-force readings (200 Hz) +- GPS prevents drift over time +- Mode detection works reliably regardless of mounting angle +- ZUPT eliminates accumulated errors at every stop + +--- + ## Future Enhancements - [ ] SD card logging for offline operation From 6249c4afbe76b600e8424d96b243899804796127 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Sun, 18 Jan 2026 18:10:08 -0800 Subject: [PATCH 20/30] Add OrientationCorrector diagnostics to web UI Display GPS-corrected orientation metrics on diagnostics page: - Pitch Correction (degrees) - learned AHRS pitch error - Roll Correction (degrees) - learned AHRS roll error - Orient Conf (pitch% / roll%) - confidence in corrections Color coding: - Green: correction < 10deg, confidence > 50% - Yellow: moderate values - Red: confidence < 10% (still learning) These metrics help validate that GPS-corrected orientation is working during test drives. --- sensors/blackbox/src/websocket_server.rs | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 822c622..440ae76 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -672,6 +672,9 @@ h1{font-size:18px;margin-bottom:16px;color:#60a5fa;letter-spacing:2px}
GPS Rejected--
+
Pitch Corr--
+
Roll Corr--
+
Orient Conf--
Yaw Bias--
Yaw Calibrated--
Tilt X/Y--
@@ -741,6 +744,16 @@ async function update(){ const rej=$('gps-rej'); rej.textContent=d.fusion.gps_rejected?'YES':'No'; rej.className='value '+(d.fusion.gps_rejected?'warn':'ok'); + // OrientationCorrector (GPS-corrected AHRS) + const pc=$('pitch-corr'); + pc.textContent=d.fusion.pitch_corr.toFixed(1)+'deg'; + pc.className='value '+(Math.abs(d.fusion.pitch_corr)<10?'ok':'warn'); + const rc=$('roll-corr'); + rc.textContent=d.fusion.roll_corr.toFixed(1)+'deg'; + rc.className='value '+(Math.abs(d.fusion.roll_corr)<10?'ok':'warn'); + const oc=$('orient-conf'); + oc.textContent=d.fusion.pitch_conf.toFixed(0)+'% / '+d.fusion.roll_conf.toFixed(0)+'%'; + oc.className='value '+(d.fusion.pitch_conf>50?'ok':(d.fusion.pitch_conf>10?'warn':'err')); $('yaw-bias').textContent=(d.fusion.yaw_bias*1000).toFixed(2)+' mrad/s'; const yc=$('yaw-cal'); yc.textContent=d.fusion.yaw_calibrated?'Yes':'No'; From a6c518dee736978a4bca4a16507c38c61655856d Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 06:55:21 -0800 Subject: [PATCH 21/30] Add color-coded thresholds to all diagnostics metrics MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Color coding (green/yellow/red) added for: - Loop Rate: >500 ok, >200 warn, <200 err - Position σ: <5m ok, <10m warn, >10m err - Velocity σ: <0.5 ok, <1.0 warn, >1.0 err - Yaw σ: <5° ok, <10° warn, >10° err - Bias X/Y: <0.3 ok, <0.5 warn, >0.5 err (absolute value) - Heap: >40KB ok, >20KB warn, <20KB err - Yaw Bias: <10 mrad/s ok, <50 warn, >50 err - Tilt X/Y: <0.3 ok, <0.5 warn, >0.5 err (max of both) Makes it easy to see at a glance if values are within spec. --- sensors/blackbox/src/websocket_server.rs | 38 ++++++++++++++++++------ 1 file changed, 29 insertions(+), 9 deletions(-) diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 440ae76..db2e2d9 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -707,7 +707,9 @@ async function update(){ const gpsEl=$('gps-rate'); gpsEl.textContent=d.sensor_rates.gps_hz.toFixed(1)+' / '+d.sensor_rates.gps_expected.toFixed(0)+' Hz'; gpsEl.className='value '+rateClass(d.sensor_rates.gps_hz,d.sensor_rates.gps_expected); - $('loop-rate').textContent=d.sensor_rates.loop_hz.toFixed(0)+' Hz'; + const lr=$('loop-rate'); + lr.textContent=d.sensor_rates.loop_hz.toFixed(0)+' Hz'; + lr.className='value '+(d.sensor_rates.loop_hz>500?'ok':(d.sensor_rates.loop_hz>200?'warn':'err')); $('zupt-rate').textContent=d.sensor_rates.zupt_per_min.toFixed(1)+'/min'; $('ekf-per-gps').textContent=d.sensor_rates.ekf_per_gps.toFixed(1); $('gps-fix').textContent=d.gps.fix?'Valid':'No Fix'; @@ -718,12 +720,24 @@ async function update(){ $('gps-hdop').className='value '+(d.gps.hdop<2?'ok':(d.gps.hdop<5?'warn':'err')); $('gps-warmup').textContent=d.gps.warmup?'Complete':'Warming up...'; $('gps-warmup').className='value '+(d.gps.warmup?'ok':'warn'); - $('pos-sigma').textContent=d.ekf.pos_sigma.toFixed(2)+' m'; - $('vel-sigma').textContent=d.ekf.vel_sigma.toFixed(2)+' m/s'; - $('yaw-sigma').textContent=d.ekf.yaw_sigma_deg.toFixed(1)+'deg'; - $('bias-x').textContent=d.ekf.bias_x.toFixed(4)+' m/s2'; - $('bias-y').textContent=d.ekf.bias_y.toFixed(4)+' m/s2'; - $('heap').textContent=(d.system.heap_free/1024).toFixed(0)+' KB'; + const ps=$('pos-sigma'); + ps.textContent=d.ekf.pos_sigma.toFixed(2)+' m'; + ps.className='value '+(d.ekf.pos_sigma<5?'ok':(d.ekf.pos_sigma<10?'warn':'err')); + const vs=$('vel-sigma'); + vs.textContent=d.ekf.vel_sigma.toFixed(2)+' m/s'; + vs.className='value '+(d.ekf.vel_sigma<0.5?'ok':(d.ekf.vel_sigma<1.0?'warn':'err')); + const ys=$('yaw-sigma'); + ys.textContent=d.ekf.yaw_sigma_deg.toFixed(1)+'deg'; + ys.className='value '+(d.ekf.yaw_sigma_deg<5?'ok':(d.ekf.yaw_sigma_deg<10?'warn':'err')); + const bx=$('bias-x'); + bx.textContent=d.ekf.bias_x.toFixed(4)+' m/s2'; + bx.className='value '+(Math.abs(d.ekf.bias_x)<0.3?'ok':(Math.abs(d.ekf.bias_x)<0.5?'warn':'err')); + const by=$('bias-y'); + by.textContent=d.ekf.bias_y.toFixed(4)+' m/s2'; + by.className='value '+(Math.abs(d.ekf.bias_y)<0.3?'ok':(Math.abs(d.ekf.bias_y)<0.5?'warn':'err')); + const hp=$('heap'); + hp.textContent=(d.system.heap_free/1024).toFixed(0)+' KB'; + hp.className='value '+(d.system.heap_free>40000?'ok':(d.system.heap_free>20000?'warn':'err')); $('tx-ok').textContent=d.system.tx_ok.toLocaleString(); const txf=$('tx-fail'); txf.textContent=d.system.tx_fail; @@ -754,11 +768,17 @@ async function update(){ const oc=$('orient-conf'); oc.textContent=d.fusion.pitch_conf.toFixed(0)+'% / '+d.fusion.roll_conf.toFixed(0)+'%'; oc.className='value '+(d.fusion.pitch_conf>50?'ok':(d.fusion.pitch_conf>10?'warn':'err')); - $('yaw-bias').textContent=(d.fusion.yaw_bias*1000).toFixed(2)+' mrad/s'; + const yb=$('yaw-bias'); + const ybVal=Math.abs(d.fusion.yaw_bias*1000); + yb.textContent=ybVal.toFixed(2)+' mrad/s'; + yb.className='value '+(ybVal<10?'ok':(ybVal<50?'warn':'err')); const yc=$('yaw-cal'); yc.textContent=d.fusion.yaw_calibrated?'Yes':'No'; yc.className='value '+(d.fusion.yaw_calibrated?'ok':'warn'); - $('tilt-xy').textContent=d.fusion.tilt_x.toFixed(3)+' / '+d.fusion.tilt_y.toFixed(3); + const txy=$('tilt-xy'); + const tiltMax=Math.max(Math.abs(d.fusion.tilt_x),Math.abs(d.fusion.tilt_y)); + txy.textContent=d.fusion.tilt_x.toFixed(3)+' / '+d.fusion.tilt_y.toFixed(3); + txy.className='value '+(tiltMax<0.3?'ok':(tiltMax<0.5?'warn':'err')); const tv=$('tilt-v'); tv.textContent=d.fusion.tilt_valid?'Yes':'No'; tv.className='value '+(d.fusion.tilt_valid?'ok':'warn'); From 984c886a3c50ad21df8df7494d464cd7e4bcfced Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 06:57:31 -0800 Subject: [PATCH 22/30] Add diagnostics color-coded thresholds documentation --- CLAUDE.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/CLAUDE.md b/CLAUDE.md index 585d708..abf4d92 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -816,6 +816,22 @@ Displayed as updates per minute (rolling average), not cumulative count. Typical - ZUPT: 0-60/min (depends on stops) - Free heap: ~60KB (stable during operation) +**Color-Coded Thresholds:** +The diagnostics page displays metrics with color coding for at-a-glance health status: + +| Metric | Green (OK) | Yellow (Warning) | Red (Error) | +|--------|------------|------------------|-------------| +| Loop Rate | > 500 Hz | > 200 Hz | < 200 Hz | +| Position σ | < 5 m | < 10 m | > 10 m | +| Velocity σ | < 0.5 m/s | < 1.0 m/s | > 1.0 m/s | +| Yaw σ | < 5° | < 10° | > 10° | +| Bias X/Y | < 0.3 m/s² | < 0.5 m/s² | > 0.5 m/s² | +| Heap | > 40 KB | > 20 KB | < 20 KB | +| Yaw Bias | < 10 mrad/s | < 50 mrad/s | > 50 mrad/s | +| Tilt X/Y | < 0.3 m/s² | < 0.5 m/s² | > 0.5 m/s² | +| Pitch/Roll Corr | < 10° | - | > 10° | +| Orient Conf | > 50% | - | < 50% | + ### udp_stream.rs - UDP Client (Station Mode) **Features:** From 2bb91ee64fdd405990c5556baa03dba9f5ee6f5e Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 07:46:04 -0800 Subject: [PATCH 23/30] Fix OrientationCorrector and improve telemetry analysis MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix Butterworth filter sample rate mismatch (200Hz -> 30Hz) The filter was misconfigured for 200Hz but called at ~30Hz telemetry rate, causing effective cutoff of ~1.5Hz instead of 10Hz and removing valid signals - Add cruise bias learning to OrientationCorrector Learns mounting offset during constant-speed driving when GPS shows ~0 accel New fields: cruise_bias, cruise_bias_sum, cruise_bias_count, cruise_time - Lower min_accel threshold from 0.5 to 0.3 m/s² for more learning opportunities - Add dt parameter to OrientationCorrector::update() for cruise timing - Add new tests for cruise bias learning behavior - Update filter tests to use correct 30Hz sample rate - Expand CSV export with fusion diagnostics columns - Enhance analyze_telemetry.py with fusion diagnostics analysis --- framework/src/fusion.rs | 257 ++++++++++++++++++----- sensors/blackbox/src/websocket_server.rs | 44 +++- tools/python/analyze_telemetry.py | 109 +++++++++- 3 files changed, 352 insertions(+), 58 deletions(-) diff --git a/framework/src/fusion.rs b/framework/src/fusion.rs index 0a41765..07ff104 100644 --- a/framework/src/fusion.rs +++ b/framework/src/fusion.rs @@ -62,11 +62,21 @@ pub struct OrientationCorrector { pitch_confidence: f32, /// Confidence in roll correction (0-1) roll_confidence: f32, + /// Cruise bias - offset learned when driving at constant speed (m/s²) + /// This catches mounting offsets that show up during cruise but not at stops + cruise_bias: f32, + /// Cruise bias accumulator for averaging + cruise_bias_sum: f32, + /// Cruise bias sample count + cruise_bias_count: u32, + /// Time spent in cruise state (seconds) + cruise_time: f32, /// EMA alpha for learning (smaller = slower but more stable) alpha: f32, /// Minimum speed for learning (m/s) - need motion for GPS accuracy min_speed: f32, - /// Minimum acceleration to trigger learning (m/s²) - need signal to learn from + /// Minimum acceleration to trigger pitch/roll learning (m/s²) + /// Below this threshold, cruise bias learning kicks in instead min_accel: f32, /// Maximum correction magnitude (radians) - safety cap max_correction: f32, @@ -81,9 +91,13 @@ impl OrientationCorrector { roll_correction: 0.0, pitch_confidence: 0.0, roll_confidence: 0.0, + cruise_bias: 0.0, + cruise_bias_sum: 0.0, + cruise_bias_count: 0, + cruise_time: 0.0, alpha: 0.05, // Slow learning for stability min_speed: 3.0, // ~11 km/h - min_accel: 0.5, // ~0.05g - need meaningful acceleration + min_accel: 0.3, // ~0.03g - lowered to learn from more events max_correction: 0.26, // ~15 degrees max update_count: 0, } @@ -98,6 +112,8 @@ impl OrientationCorrector { /// * `lat_gps` - GPS-derived lateral accel (from centripetal: speed × yaw_rate) /// * `speed` - Vehicle speed in m/s /// * `is_gps_fresh` - Whether GPS data is recent and valid + /// * `dt` - Time step in seconds (for cruise learning timing) + #[allow(clippy::too_many_arguments)] pub fn update( &mut self, lon_imu: f32, @@ -106,16 +122,22 @@ impl OrientationCorrector { lat_gps: f32, speed: f32, is_gps_fresh: bool, + dt: f32, ) { // Only learn when conditions are good if speed < self.min_speed || !is_gps_fresh { + self.cruise_time = 0.0; // Reset cruise accumulator + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; return; } + // Apply current cruise bias before comparison + let lon_imu_corrected = lon_imu - self.cruise_bias; + let lon_error = lon_imu_corrected - lon_gps; + // Learn pitch correction from longitudinal acceleration difference - // During forward acceleration: pitch error → wrong ax_earth - // pitch_error ≈ (ax_imu - ax_gps) / G - let lon_error = lon_imu - lon_gps; + // During acceleration: pitch error → wrong ax_earth if lon_gps.abs() > self.min_accel { // Signal present - can learn pitch let pitch_innovation = lon_error / G; @@ -128,6 +150,32 @@ impl OrientationCorrector { // Update confidence self.update_count = self.update_count.saturating_add(1); self.pitch_confidence = (self.update_count as f32 / 100.0).min(1.0); + + // Reset cruise accumulator when accelerating + self.cruise_time = 0.0; + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; + } else { + // Cruising (lon_gps near zero) - learn cruise bias + // If GPS says ~0 accel but IMU shows offset, that's mounting bias + self.cruise_time += dt; + self.cruise_bias_sum += lon_imu_corrected; // Should be ~0 when cruising + self.cruise_bias_count += 1; + + // After 2 seconds of stable cruising, update cruise bias + if self.cruise_time >= 2.0 && self.cruise_bias_count > 30 { + let measured_bias = self.cruise_bias_sum / self.cruise_bias_count as f32; + + // EMA update for cruise bias (slower alpha for stability) + self.cruise_bias = 0.9 * self.cruise_bias + 0.1 * measured_bias; + // Clamp cruise bias to reasonable range (±1.5 m/s² = ±0.15g) + self.cruise_bias = self.cruise_bias.clamp(-1.5, 1.5); + + // Reset accumulator + self.cruise_time = 0.0; + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; + } } // Learn roll correction from lateral acceleration difference @@ -144,6 +192,11 @@ impl OrientationCorrector { } } + /// Get current cruise bias (m/s²) for diagnostics + pub fn get_cruise_bias(&self) -> f32 { + self.cruise_bias + } + /// Apply corrections to AHRS orientation /// /// Returns corrected (pitch_deg, roll_deg) for use in gravity removal @@ -181,6 +234,10 @@ impl OrientationCorrector { self.roll_correction = 0.0; self.pitch_confidence = 0.0; self.roll_confidence = 0.0; + self.cruise_bias = 0.0; + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; + self.cruise_time = 0.0; self.update_count = 0; } } @@ -214,7 +271,8 @@ pub struct FusionConfig { /// Removes engine vibration (20-100Hz) while passing driving dynamics (0-3Hz) /// Research: ArduPilot uses 10Hz, academic papers suggest 1-5Hz for vehicle dynamics pub lon_filter_cutoff: f32, - /// Sample rate for longitudinal filter (Hz) - must match telemetry rate + /// Sample rate for longitudinal filter (Hz) - must match ACTUAL call rate of process_imu() + /// CRITICAL: This is telemetry rate (~30Hz), NOT IMU rate (200Hz)! pub lon_sample_rate: f32, } @@ -247,8 +305,9 @@ impl Default for FusionConfig { // Butterworth filter for IMU // 10 Hz cutoff: passes driving dynamics, removes vibration lon_filter_cutoff: 10.0, - // CRITICAL: Must match actual IMU sample rate, NOT telemetry rate! - lon_sample_rate: 200.0, + // CRITICAL: Must match actual process_imu() call rate (telemetry rate ~30Hz) + // NOT the IMU hardware rate (200Hz)! + lon_sample_rate: 30.0, } } } @@ -802,6 +861,7 @@ impl SensorFusion { gps_lat, // Centripetal (truth for lateral) speed, gps_fresh, + dt, // Time step for cruise bias learning ); } } @@ -1273,25 +1333,26 @@ mod tests { } #[test] - fn test_biquad_filter_removes_engine_vibration() { - // Simulate engine vibration at 30Hz with 0.1g amplitude (~1 m/s²) - // The 15Hz Butterworth filter should attenuate this by ~12dB + fn test_biquad_filter_removes_high_frequency_noise() { + // Simulate noise at 12Hz with 0.1g amplitude (~1 m/s²) + // The 10Hz cutoff Butterworth filter should attenuate this + // (12Hz is above 10Hz cutoff but below 15Hz Nyquist for 30Hz sample rate) let config = FusionConfig::default(); let mut fusion = SensorFusion::new(config); - let vibration_freq = 30.0; // Hz - typical engine vibration - let vibration_amp = 1.0; // m/s² (~0.1g) - let sample_rate = 200.0; // Hz - IMU rate (must match config!) + let noise_freq = 12.0; // Hz - above 10Hz cutoff, below 15Hz Nyquist + let noise_amp = 1.0; // m/s² (~0.1g) + let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) - // Run for 2 seconds to let filter settle + // Run for 3 seconds to let filter settle let mut max_output: f32 = 0.0; - for i in 0..400 { + for i in 0..90 { let t = i as f32 / sample_rate; - // Simulate vibrating IMU input in body frame - let vibration = vibration_amp * (2.0 * core::f32::consts::PI * vibration_freq * t).sin(); + // Simulate noisy IMU input in body frame + let noise = noise_amp * (2.0 * core::f32::consts::PI * noise_freq * t).sin(); let (lon, _lat) = fusion.process_imu( - vibration, // ax_raw = vibrating + noise, // ax_raw = noisy 0.0, // ay_raw = 0 G, // az_raw = gravity 0.0, // roll = level @@ -1299,22 +1360,22 @@ mod tests { 0.0, // yaw = 0 (heading east) 10.0, // speed 0.0, // yaw_rate = 0 - 0.005, // dt = 5ms (200Hz) + 0.033, // dt = 33ms (~30Hz) false, // not stationary ); - // After settling (1 second = 200 samples), check output - if i > 200 { + // After settling (1 second = 30 samples), check output + if i > 30 { max_output = max_output.max(lon.abs()); } } - // 30Hz vibration should be attenuated by ~12dB at 15Hz cutoff - // That means 1 m/s² input → ~0.25 m/s² output (or less) - // Being conservative, check it's below 0.4 m/s² (~0.04g) + // 12Hz noise should be attenuated by the 10Hz cutoff filter + // Expect ~6dB attenuation: 1 m/s² input → ~0.5 m/s² output + // Being conservative, check it's below 0.7 m/s² assert!( - max_output < 0.4, - "30Hz vibration should be attenuated: got {} m/s²", + max_output < 0.7, + "12Hz noise should be attenuated: got {} m/s²", max_output ); } @@ -1322,17 +1383,17 @@ mod tests { #[test] fn test_biquad_filter_passes_driving_dynamics() { // Simulate a braking event at 1Hz (typical vehicle dynamics) - // The 15Hz filter should pass this with minimal attenuation + // The 10Hz cutoff filter should pass this with minimal attenuation let config = FusionConfig::default(); let mut fusion = SensorFusion::new(config); let dynamics_freq = 1.0; // Hz - braking event let dynamics_amp = 3.0; // m/s² (~0.3g braking) - let sample_rate = 200.0; // Hz - IMU rate (must match config!) + let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) - // Run for 3 seconds + // Run for 5 seconds to let filter settle let mut max_output: f32 = 0.0; - for i in 0..600 { + for i in 0..150 { let t = i as f32 / sample_rate; let signal = dynamics_amp * (2.0 * core::f32::consts::PI * dynamics_freq * t).sin(); @@ -1345,21 +1406,21 @@ mod tests { 0.0, // yaw 10.0, // speed 0.0, // yaw_rate - 0.005, // dt = 5ms (200Hz) + 0.033, // dt = 33ms (~30Hz) false, ); - // After settling (1 second = 200 samples), check output - if i > 200 { + // After settling (1 second = 30 samples), check output + if i > 30 { max_output = max_output.max(lon.abs()); } } - // 1Hz should pass through with >90% amplitude - // 3 m/s² input → >2.7 m/s² output + // 1Hz should pass through with >80% amplitude at 10Hz cutoff + // 3 m/s² input → >2.4 m/s² output assert!( - max_output > 2.5, - "1Hz driving dynamics should pass through: got {} m/s², expected >2.5", + max_output > 2.4, + "1Hz driving dynamics should pass through: got {} m/s², expected >2.4", max_output ); } @@ -1369,10 +1430,11 @@ mod tests { // Verify critical configuration values let config = FusionConfig::default(); - // Filter must be configured for IMU rate (200Hz), NOT telemetry rate (20Hz) + // Filter must be configured for TELEMETRY rate (~30Hz), not IMU hardware rate + // process_imu() is called at telemetry rate, not IMU sample rate! assert_eq!( - config.lon_sample_rate, 200.0, - "lon_sample_rate must be 200Hz (IMU rate)" + config.lon_sample_rate, 30.0, + "lon_sample_rate must be 30Hz (telemetry rate, not IMU hardware rate)" ); // Filter cutoff should be 10Hz for smoother output @@ -1707,6 +1769,7 @@ mod tests { 0.0, // lat_gps speed, true, // GPS fresh + 0.033, // dt (~30Hz) ); } @@ -1746,6 +1809,7 @@ mod tests { lat_gps, // GPS lateral (centripetal) speed, true, + 0.005, // dt = 5ms (200Hz) ); } @@ -1774,6 +1838,7 @@ mod tests { 0.0, low_speed, true, + 0.005, // dt = 5ms ); } @@ -1792,22 +1857,32 @@ mod tests { fn test_orientation_corrector_ignores_small_accel() { let mut corrector = OrientationCorrector::new(); - // When not accelerating (GPS shows ~0), no signal to learn from + // When not accelerating (GPS shows ~0), no signal to learn from for pitch + // BUT now this should learn cruise_bias instead! for _ in 0..100 { corrector.update( - 0.3, // Small IMU value (noise) + 0.3, // Small IMU value (offset) 0.0, // GPS shows zero (cruising) 0.0, 0.0, 15.0, // Good speed true, + 0.05, // dt = 50ms (shorter loop, 2s = 40 iterations, we do 100) ); } - // Should NOT have learned - GPS accel below min_accel threshold + // Pitch correction should NOT have learned - GPS accel below min_accel threshold assert_eq!( corrector.pitch_correction, 0.0, - "Should not learn when GPS accel < min_accel" + "Should not learn pitch when GPS accel < min_accel" + ); + + // But cruise_bias SHOULD have been learned! (new behavior) + // IMU shows 0.3 m/s² offset during cruise, cruise bias should converge toward it + assert!( + corrector.cruise_bias.abs() > 0.01, + "Should learn cruise_bias when cruising, got {}", + corrector.cruise_bias ); } @@ -1820,7 +1895,7 @@ mod tests { let lon_gps = 3.0; // 7 m/s² difference = huge pitch error for _ in 0..1000 { - corrector.update(lon_imu, lon_gps, 0.0, 0.0, 15.0, true); + corrector.update(lon_imu, lon_gps, 0.0, 0.0, 15.0, true, 0.005); } // Should be clamped at max_correction (0.26 rad = 15°) @@ -1864,7 +1939,7 @@ mod tests { // Learn some corrections for _ in 0..200 { - corrector.update(5.0, 3.0, 3.0, 2.0, 15.0, true); + corrector.update(5.0, 3.0, 3.0, 2.0, 15.0, true, 0.005); } assert!(corrector.pitch_correction != 0.0); @@ -1891,6 +1966,7 @@ mod tests { 0.0, 15.0, false, // GPS NOT fresh + 0.005, ); } @@ -1899,4 +1975,91 @@ mod tests { "Should not learn when GPS not fresh" ); } + + #[test] + fn test_orientation_corrector_cruise_bias_learning() { + // Test that cruise bias is learned during constant-speed driving + // when GPS shows ~0 acceleration but IMU has a consistent offset + let mut corrector = OrientationCorrector::new(); + + // Simulate 5 seconds of cruising at constant speed + // IMU shows 0.5 m/s² offset (mounting error), GPS shows ~0 (cruising) + let imu_offset = 0.5; // m/s² - consistent offset during cruise + let dt = 0.05; // 50ms = 20Hz, so 2s = 40 samples + + // Run for 5 seconds (100 iterations at 50ms each) + for _ in 0..100 { + corrector.update( + imu_offset, // IMU shows offset + 0.0, // GPS shows zero (cruising) + 0.0, + 0.0, + 15.0, // 54 km/h - above min_speed + true, + dt, + ); + } + + // Cruise bias should have been learned + // With slow EMA (alpha=0.1) and 2s update cycle, after 5s we get ~2 updates + // First update: bias = 0 + 0.1 * 0.5 = 0.05 + // Second update: corrected = 0.5 - 0.05 = 0.45, bias = 0.05 * 0.9 + 0.1 * 0.45 = 0.09 + let cruise_bias = corrector.get_cruise_bias(); + assert!( + cruise_bias.abs() > 0.05, + "Cruise bias should be learning (>0.05), got {}", + cruise_bias + ); + assert!( + cruise_bias > 0.0, + "Cruise bias should be positive (learning toward IMU offset): got {}", + cruise_bias + ); + + // Pitch correction should NOT be learned (no GPS accel signal) + assert!( + corrector.pitch_correction.abs() < 0.01, + "Pitch should not change during cruise" + ); + } + + #[test] + fn test_orientation_corrector_cruise_bias_resets_on_accel() { + // Test that cruise bias accumulator resets when acceleration is detected + let mut corrector = OrientationCorrector::new(); + + // First, build up some cruise bias + for _ in 0..50 { + corrector.update(0.3, 0.0, 0.0, 0.0, 15.0, true, 0.05); + } + + // Now accelerate - this should reset the cruise accumulator + // but preserve the learned cruise_bias from before + let bias_before = corrector.get_cruise_bias(); + + for _ in 0..10 { + corrector.update( + 3.5, // IMU shows acceleration + 3.0, // GPS also shows acceleration + 0.0, + 0.0, + 15.0, + true, + 0.05, + ); + } + + // Cruise bias should be preserved (not cleared by acceleration) + let bias_after = corrector.get_cruise_bias(); + assert!( + (bias_after - bias_before).abs() < 0.1, + "Cruise bias should be preserved during acceleration" + ); + + // But pitch should now be learning + assert!( + corrector.pitch_correction.abs() > 0.001, + "Pitch should be learning during acceleration" + ); + } } diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index db2e2d9..2b46153 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -305,14 +305,14 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0
Min Speed2.0 m/s
-
Accel0.10g
-
Acc Exit0.05g
-
Brake0.18g
-
Brk Exit0.09g
-
Lateral0.12g
-
Lat Exit0.06g
-
Yaw0.050r/s
-
Min Spd2.0m/s
+
Accel0.10g
+
Acc Exit0.05g
+
Brake0.18g
+
Brk Exit0.09g
+
Lateral0.12g
+
Lat Exit0.06g
+
Yaw0.050r/s
+
Min Spd2.0m/s
@@ -328,6 +328,9 @@ let trail=[],maxL=0,maxR=0,maxA=0,maxB=0,maxSpd=0; let speed_ema=0; let sessionStart=Date.now(); let currentPreset='city'; +// Fusion diagnostics for CSV export (fetched periodically during recording) +let fusion={lon_imu:0,lon_gps:0,gps_wt:0,pitch_c:0,pitch_cf:0,roll_c:0,roll_cf:0,tilt_x:0,tilt_y:0}; +let fusionPoll=0; const $=id=>document.getElementById(id); const cv=$('gfc'),ctx=cv.getContext('2d'); const CX=70,CY=70,R=55,SCL=R/2; @@ -438,7 +441,7 @@ if(lng>0&&lng>maxA){maxA=lng;$('maxA').textContent=maxA.toFixed(2)} if(lng<0&&Math.abs(lng)>maxB){maxB=Math.abs(lng);$('maxB').textContent=maxB.toFixed(2)} drawG(); cnt++; -if(rec)data.push({t:Date.now(),sp,ax,ay,wz,mo,latg,lng,lat,lon,gpsOk}); +if(rec)data.push({t:Date.now(),sp,ax,ay,wz,mo,latg,lng,lat,lon,gpsOk,...fusion}); } // HTTP polling - self-scheduling for maximum throughput @@ -452,11 +455,32 @@ const b=atob(j.data),a=new Uint8Array(b.length); for(let i=0;i=5){fusionPoll=0;fetchFusion()} } setTimeout(poll,33); // Poll at ~30Hz to match ESP32 telemetry rate }catch(e){$('dot').className='dot';$('stxt').textContent='Offline';setTimeout(poll,500)} // Retry slower on error } +// Fetch fusion diagnostics for CSV export (non-blocking) +async function fetchFusion(){ +try{ +const r=await fetch('/api/diagnostics'); +const d=await r.json(); +if(d.fusion){ +fusion.lon_imu=d.fusion.lon_filtered||0; +fusion.lon_gps=d.fusion.gps_accel||0; +fusion.gps_wt=d.fusion.gps_weight||0; +fusion.pitch_c=d.fusion.pitch_corr||0; +fusion.pitch_cf=d.fusion.pitch_conf||0; +fusion.roll_c=d.fusion.roll_corr||0; +fusion.roll_cf=d.fusion.roll_conf||0; +fusion.tilt_x=d.fusion.tilt_x||0; +fusion.tilt_y=d.fusion.tilt_y||0; +} +}catch(e){} +} + // Reset button: resets G max, speed max, timer, and triggers calibration $('rstg').onclick=resetAll; $('gfc').ondblclick=resetGMax; @@ -474,7 +498,7 @@ if(data.length){const s=JSON.parse(localStorage.getItem('bb')||'[]');s.unshift({ localStorage.setItem('bb',JSON.stringify(s.slice(0,10)));alert('Saved '+data.length+' pts')}}}; $('exp').onclick=()=>{const s=JSON.parse(localStorage.getItem('bb')||'[]');if(!s.length)return alert('No data'); -let c='time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid\n';s[0].d.forEach(r=>{c+=r.t+','+r.sp+','+r.ax+','+r.ay+','+r.wz+','+r.mo+','+r.latg+','+r.lng+','+(r.lat||0)+','+(r.lon||0)+','+(r.gpsOk||0)+'\n'}); +let c='time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid,lon_imu,lon_gps,gps_weight,pitch_corr,pitch_conf,roll_corr,roll_conf,tilt_x,tilt_y\n';s[0].d.forEach(r=>{c+=r.t+','+r.sp+','+r.ax+','+r.ay+','+r.wz+','+r.mo+','+r.latg+','+r.lng+','+(r.lat||0)+','+(r.lon||0)+','+(r.gpsOk||0)+','+(r.lon_imu||0).toFixed(4)+','+(r.lon_gps||0).toFixed(4)+','+(r.gps_wt||0).toFixed(2)+','+(r.pitch_c||0).toFixed(2)+','+(r.pitch_cf||0).toFixed(1)+','+(r.roll_c||0).toFixed(2)+','+(r.roll_cf||0).toFixed(1)+','+(r.tilt_x||0).toFixed(4)+','+(r.tilt_y||0).toFixed(4)+'\n'}); const b=new Blob([c],{type:'text/csv'}),u=URL.createObjectURL(b),a=document.createElement('a');a.href=u;a.download='blackbox.csv';a.click()}; // Settings functions diff --git a/tools/python/analyze_telemetry.py b/tools/python/analyze_telemetry.py index 32f4066..b10157f 100644 --- a/tools/python/analyze_telemetry.py +++ b/tools/python/analyze_telemetry.py @@ -4,7 +4,10 @@ Usage: python analyze_telemetry.py -CSV format expected: +CSV format expected (new format with fusion diagnostics): +time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid,lon_imu,lon_gps,gps_weight,pitch_corr,pitch_conf,roll_corr,roll_conf,tilt_x,tilt_y + +Also supports legacy format: time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid """ @@ -22,10 +25,14 @@ def analyze_telemetry(filename): print("No data in file") return + # Detect if we have fusion columns (new format) + has_fusion = 'lon_imu' in rows[0] and 'lon_gps' in rows[0] + print(f"{'='*60}") print(f"TELEMETRY ANALYSIS: {filename}") print(f"{'='*60}") print(f"Total samples: {len(rows)}") + print(f"Format: {'Extended (with fusion diagnostics)' if has_fusion else 'Legacy'}") print() # === TIMING ANALYSIS === @@ -82,6 +89,78 @@ def analyze_telemetry(filename): f"(bias indicator - should be ~0)") print() + # === FUSION DIAGNOSTICS (if available) === + if has_fusion: + print("--- FUSION DIAGNOSTICS ---") + + # IMU vs GPS acceleration comparison + lon_imu_vals = [float(r['lon_imu']) for r in rows] + lon_gps_vals = [float(r['lon_gps']) for r in rows] + gps_weights = [float(r['gps_weight']) for r in rows] + + # Filter out samples where GPS accel is 0 (stale/unavailable) + valid_gps = [(imu, gps, w) for imu, gps, w in zip(lon_imu_vals, lon_gps_vals, gps_weights) if abs(gps) > 0.001] + + if valid_gps: + imu_valid = [v[0] for v in valid_gps] + gps_valid = [v[1] for v in valid_gps] + + print(f"lon_imu range: {min(lon_imu_vals):.3f} to {max(lon_imu_vals):.3f} m/s²") + print(f"lon_gps range: {min(gps_valid):.3f} to {max(gps_valid):.3f} m/s²") + + # IMU vs GPS error + errors = [i - g for i, g in zip(imu_valid, gps_valid)] + mean_error = statistics.mean(errors) + print(f"IMU-GPS error: mean={mean_error:.3f} m/s² ({mean_error/9.81:.3f}g), " + f"std={statistics.stdev(errors):.3f} m/s²") + + # Correlation between IMU and GPS + if len(imu_valid) >= 10: + mean_imu = statistics.mean(imu_valid) + mean_gps = statistics.mean(gps_valid) + numerator = sum((i - mean_imu) * (g - mean_gps) for i, g in zip(imu_valid, gps_valid)) + denom_imu = sum((i - mean_imu)**2 for i in imu_valid) ** 0.5 + denom_gps = sum((g - mean_gps)**2 for g in gps_valid) ** 0.5 + if denom_imu > 0 and denom_gps > 0: + corr = numerator / (denom_imu * denom_gps) + print(f"IMU-GPS correlation: {corr:.3f} (want >0.7)") + + # GPS weight distribution + avg_weight = statistics.mean(gps_weights) + high_gps = sum(1 for w in gps_weights if w > 0.7) + mid_gps = sum(1 for w in gps_weights if 0.3 <= w <= 0.7) + low_gps = sum(1 for w in gps_weights if w < 0.3) + print(f"GPS weight: mean={avg_weight:.2f}, high(>0.7)={100*high_gps/len(rows):.0f}%, " + f"mid={100*mid_gps/len(rows):.0f}%, low(<0.3)={100*low_gps/len(rows):.0f}%") + + # OrientationCorrector status + pitch_corrs = [float(r['pitch_corr']) for r in rows] + pitch_confs = [float(r['pitch_conf']) for r in rows] + roll_corrs = [float(r['roll_corr']) for r in rows] + roll_confs = [float(r['roll_conf']) for r in rows] + + print(f"Pitch correction: {statistics.mean(pitch_corrs):.1f}° (range {min(pitch_corrs):.1f}° to {max(pitch_corrs):.1f}°)") + print(f"Pitch confidence: {statistics.mean(pitch_confs):.0f}% (range {min(pitch_confs):.0f}% to {max(pitch_confs):.0f}%)") + print(f"Roll correction: {statistics.mean(roll_corrs):.1f}° (range {min(roll_corrs):.1f}° to {max(roll_corrs):.1f}°)") + print(f"Roll confidence: {statistics.mean(roll_confs):.0f}% (range {min(roll_confs):.0f}% to {max(roll_confs):.0f}%)") + + # Tilt offsets + tilt_x = [float(r['tilt_x']) for r in rows] + tilt_y = [float(r['tilt_y']) for r in rows] + print(f"Tilt offset: X={statistics.mean(tilt_x):.3f}, Y={statistics.mean(tilt_y):.3f} m/s²") + print() + + # Confidence assessment + avg_pitch_conf = statistics.mean(pitch_confs) + if avg_pitch_conf < 30: + print("⚠️ LOW PITCH CONFIDENCE: OrientationCorrector hasn't learned enough") + print(" Need more acceleration/braking events for learning") + elif avg_pitch_conf < 70: + print("⚠️ MODERATE PITCH CONFIDENCE: OrientationCorrector still learning") + else: + print("✓ HIGH PITCH CONFIDENCE: OrientationCorrector well-calibrated") + print() + # === RAW ACCELEROMETER === print("--- RAW ACCELEROMETER (ax in m/s²) ---") ax_vals = [float(r['ax']) for r in rows] @@ -242,6 +321,16 @@ def analyze_telemetry(filename): if tp_brake + fp_brake > 0 and fp_brake / (tp_brake + fp_brake) > 0.3: issues.append(f"High BRAKE false positive rate: {100*fp_brake/(tp_brake+fp_brake):.0f}%") + # Check OrientationCorrector (if fusion data available) + if has_fusion: + avg_pitch_conf = statistics.mean([float(r['pitch_conf']) for r in rows]) + if avg_pitch_conf < 30: + issues.append(f"Low OrientationCorrector confidence: {avg_pitch_conf:.0f}% (need more driving to learn)") + + avg_gps_weight = statistics.mean([float(r['gps_weight']) for r in rows]) + if avg_gps_weight > 0.8: + issues.append(f"High GPS weight: {avg_gps_weight:.0f}% (IMU correction not trusted yet)") + if issues: print("ISSUES DETECTED:") for issue in issues: @@ -249,6 +338,24 @@ def analyze_telemetry(filename): else: print("No major issues detected") + # Recommendations + print() + print("--- RECOMMENDATIONS ---") + if has_fusion: + avg_pitch_conf = statistics.mean([float(r['pitch_conf']) for r in rows]) + if avg_pitch_conf < 50: + print("• Drive more with varied acceleration/braking to train OrientationCorrector") + + if stable_lon: + bias = statistics.mean(stable_lon) + if bias > 0.05: + print(f"• Consider raising ACCEL threshold by ~{bias:.2f}g to compensate for positive bias") + print(f"• Consider lowering BRAKE threshold by ~{bias:.2f}g to improve brake detection") + elif bias < -0.05: + print(f"• Consider lowering ACCEL threshold by ~{abs(bias):.2f}g to compensate for negative bias") + else: + print("• Re-record with latest firmware to get fusion diagnostics for detailed analysis") + print(f"\n{'='*60}") if __name__ == '__main__': From 53ea19d1f94df6c3f9b1c2534e75670d711d3749 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 09:15:06 -0800 Subject: [PATCH 24/30] Update city preset thresholds for better detection City preset changes: - acc: unchanged at 0.10g (cruise bias learning handles offset) - brake: 0.18 -> 0.25g (less aggressive, reduce false braking) - lat: 0.12 -> 0.10g (more sensitive corner detection) - yaw: 0.05 -> 0.04 rad/s (more sensitive corner detection) Also updates: - mode.rs default ModeConfig to match new city preset - Slider default values in UI to match - Summary display defaults - Test values for combined brake+corner --- framework/src/mode.rs | 12 ++++++------ sensors/blackbox/src/websocket_server.rs | 20 ++++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/framework/src/mode.rs b/framework/src/mode.rs index cf2d16b..319286a 100644 --- a/framework/src/mode.rs +++ b/framework/src/mode.rs @@ -131,11 +131,11 @@ impl Default for ModeConfig { min_speed: 2.0, // ~7 km/h - must be moving for accel/brake/corner acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) acc_exit: 0.05, // exit when below 0.05g - brake_thr: -0.18, // -0.18g - city driving (normal braking) - brake_exit: -0.09, // exit when above -0.09g - lat_thr: 0.12, // 0.12g lateral - city turns - lat_exit: 0.06, // exit when below 0.06g - yaw_thr: 0.05, // ~2.9°/s yaw rate + brake_thr: -0.25, // -0.25g - less aggressive brake detection + brake_exit: -0.12, // exit when above -0.12g + lat_thr: 0.10, // 0.10g lateral - more sensitive corner detection + lat_exit: 0.05, // exit when below 0.05g + yaw_thr: 0.04, // ~2.3°/s yaw rate - more sensitive alpha: DEFAULT_MODE_ALPHA, } } @@ -474,7 +474,7 @@ mod tests { let mut classifier = ModeClassifier::new(); // Simulate braking while cornering (corner entry) - let ax = -0.25 * G; // 0.25g braking + let ax = -0.35 * G; // 0.35g braking (above -0.25g threshold) let ay = -0.15 * G; // 0.15g lateral (left turn) let yaw = 0.0; let wz = -0.08; // -0.08 rad/s yaw rate (left) diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 2b46153..138aed6 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -299,19 +299,19 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0
Accel0.10/0.05g
-
Brake0.18/0.09g
-
Lateral0.12/0.06g
-
Yaw0.05 r/s
+
Brake0.25/0.12g
+
Lateral0.10/0.05g
+
Yaw0.04 r/s
Min Speed2.0 m/s
Accel0.10g
Acc Exit0.05g
-
Brake0.18g
-
Brk Exit0.09g
-
Lateral0.12g
-
Lat Exit0.06g
-
Yaw0.050r/s
+
Brake0.25g
+
Brk Exit0.12g
+
Lateral0.10g
+
Lat Exit0.05g
+
Yaw0.040r/s
Min Spd2.0m/s
@@ -343,8 +343,8 @@ const CX=70,CY=70,R=55,SCL=R/2; const PRESETS={ track:{acc:0.35,acc_exit:0.17,brake:0.55,brake_exit:0.27,lat:0.50,lat_exit:0.25,yaw:0.15,min_speed:4.0,desc:'Racing/track days'}, canyon:{acc:0.22,acc_exit:0.11,brake:0.35,brake_exit:0.17,lat:0.28,lat_exit:0.14,yaw:0.10,min_speed:3.0,desc:'Spirited mountain roads'}, -city:{acc:0.10,acc_exit:0.05,brake:0.18,brake_exit:0.09,lat:0.12,lat_exit:0.06,yaw:0.05,min_speed:2.0,desc:'Daily street driving'}, -highway:{acc:0.12,acc_exit:0.06,brake:0.22,brake_exit:0.11,lat:0.14,lat_exit:0.07,yaw:0.04,min_speed:5.0,desc:'Highway cruising'} +city:{acc:0.10,acc_exit:0.05,brake:0.25,brake_exit:0.12,lat:0.10,lat_exit:0.05,yaw:0.04,min_speed:2.0,desc:'Daily street driving'}, +highway:{acc:0.12,acc_exit:0.06,brake:0.25,brake_exit:0.12,lat:0.12,lat_exit:0.06,yaw:0.04,min_speed:5.0,desc:'Highway cruising'} }; function fmtTime(ms){const s=Math.floor(ms/1000),m=Math.floor(s/60);return String(m).padStart(2,'0')+':'+String(s%60).padStart(2,'0')} From e396fd007792b302d02b3be35e2921bd699ec44d Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 12:15:56 -0800 Subject: [PATCH 25/30] Fix GPS processing and sync threshold presets MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix critical bug: process_gps() now called with scalar speed on every new fix, not just when velocity_enu available (enables OrientationCorrector) - Fix lon_sample_rate: 20Hz → 30Hz to match TelemetryConfig default (33ms) - Fix gps_max_age: 0.2s → 0.4s for 25Hz GPS with processing margin - Lower brake thresholds across all presets to account for mounting bias - Sync ModeSettings::default() and HTML slider defaults with mode.rs - Update CLAUDE.md preset documentation - Enhance docs/index.html JSON-LD metadata for LLM discovery --- CLAUDE.md | 12 ++-- docs/index.html | 87 +++++++++++++++++++++--- framework/src/mode.rs | 8 +-- sensors/blackbox/src/main.rs | 40 +++++++---- sensors/blackbox/src/websocket_server.rs | 26 +++---- 5 files changed, 127 insertions(+), 46 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index abf4d92..ba662cf 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -304,10 +304,10 @@ After 5 consecutive stationary detections: | Preset | Accel | Brake | Lateral | Yaw | Min Speed | Use Case | |--------|-------|-------|---------|-----|-----------|----------| -| Track | 0.30g | 0.50g | 0.50g | 0.15 rad/s | 3.0 m/s | Racing, autocross | -| Canyon | 0.20g | 0.35g | 0.30g | 0.10 rad/s | 2.5 m/s | Spirited twisty roads | -| City (default) | 0.10g | 0.18g | 0.12g | 0.05 rad/s | 2.0 m/s | Daily driving | -| Highway | 0.08g | 0.15g | 0.10g | 0.04 rad/s | 4.0 m/s | Cruise, subtle inputs | +| Track | 0.35g | 0.35g | 0.50g | 0.15 rad/s | 4.0 m/s | Racing, autocross | +| Canyon | 0.22g | 0.22g | 0.28g | 0.10 rad/s | 3.0 m/s | Spirited twisty roads | +| City (default) | 0.10g | 0.15g | 0.10g | 0.04 rad/s | 2.0 m/s | Daily driving | +| Highway | 0.12g | 0.15g | 0.12g | 0.04 rad/s | 5.0 m/s | Cruise, subtle inputs | | Custom | User-defined | | | | | Fine-tuning via sliders | Exit thresholds are 50% of entry values for hysteresis (prevents oscillation). @@ -543,8 +543,8 @@ Result: acceleration due to motion only (no gravity component). Independent hysteresis detection: ACCEL: entry at 0.10g → exit at 0.05g (from blended lon accel) -BRAKE: entry at -0.18g → exit at -0.09g (from blended lon accel) -CORNER: entry at 0.12g lat + 0.05rad/s yaw → exit at 0.06g lat +BRAKE: entry at -0.15g → exit at -0.08g (from blended lon accel) +CORNER: entry at 0.10g lat + 0.04rad/s yaw → exit at 0.05g lat Combined states: - ACCEL + CORNER = corner exit (trail throttle) diff --git a/docs/index.html b/docs/index.html index c6c46d5..88a484d 100644 --- a/docs/index.html +++ b/docs/index.html @@ -21,29 +21,100 @@ - + + + + diff --git a/framework/src/mode.rs b/framework/src/mode.rs index 319286a..00e199f 100644 --- a/framework/src/mode.rs +++ b/framework/src/mode.rs @@ -131,11 +131,11 @@ impl Default for ModeConfig { min_speed: 2.0, // ~7 km/h - must be moving for accel/brake/corner acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) acc_exit: 0.05, // exit when below 0.05g - brake_thr: -0.25, // -0.25g - less aggressive brake detection - brake_exit: -0.12, // exit when above -0.12g - lat_thr: 0.10, // 0.10g lateral - more sensitive corner detection + brake_thr: -0.15, // -0.15g - sensitive brake detection (compensates for mounting bias) + brake_exit: -0.08, // exit when above -0.08g + lat_thr: 0.10, // 0.10g lateral - sensitive corner detection lat_exit: 0.05, // exit when below 0.05g - yaw_thr: 0.04, // ~2.3°/s yaw rate - more sensitive + yaw_thr: 0.04, // ~2.3°/s yaw rate - sensitive alpha: DEFAULT_MODE_ALPHA, } } diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 8589a15..2cf12fb 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -461,15 +461,18 @@ fn main() { let fusion_config = FusionConfig { gps_high_rate: 20.0, gps_medium_rate: 10.0, - gps_max_age: 0.2, // GPS older than 200ms is stale → fall back to IMU + // GPS staleness threshold - must be > GPS interval + processing delay + // At 5Hz GPS (200ms) with ~20Hz telemetry, 400ms gives safe margin + gps_max_age: 0.4, // GPS weights based on orientation correction confidence: gps_weight_high: 0.3, // 30% GPS when correction is very confident gps_weight_medium: 0.6, // 60% GPS when moderately confident gps_weight_low: 1.0, // 100% GPS when not confident (no correction yet) tilt_learn_time: 3.0, // Butterworth filter to remove engine vibration from IMU + // Sample rate must match ACTUAL process_imu() call rate (telemetry rate) lon_filter_cutoff: 10.0, - lon_sample_rate: 200.0, + lon_sample_rate: 30.0, // 33ms telemetry interval = ~30Hz (from TelemetryConfig::default) }; let mut sensor_fusion = SensorFusion::new(fusion_config); @@ -728,11 +731,28 @@ fn main() { if sensors.poll_gps() { // Count GPS rate only when a NEW valid RMC fix is received // (not on every GGA/GSA sentence while last_fix.valid is still true) - if sensors.gps_parser.take_new_fix() { + let is_new_fix = sensors.gps_parser.take_new_fix(); + if is_new_fix { diagnostics.record_gps_fix(); } - // Check if warmup complete (has reference point) AND fix is valid + // ALWAYS feed GPS speed to sensor fusion when we have a new fix + // GPS acceleration only needs scalar speed - doesn't require full position validity + // This ensures OrientationCorrector can learn even during brief GPS hiccups + if sensors.gps_parser.is_warmed_up() && is_new_fix { + let gps_speed = sensors.gps_parser.last_fix().speed; + let time_s = now_ms as f32 / 1000.0; + sensor_fusion.process_gps(gps_speed, time_s); + + // Feed GPS heading to yaw rate calibrator (only valid at speed) + // Course over ground is unreliable below ~5 m/s + if gps_speed > 5.0 { + let course = sensors.gps_parser.last_fix().course; + sensor_fusion.process_gps_heading(course); + } + } + + // Check if warmup complete (has reference point) AND fix is valid for EKF updates if sensors.gps_parser.is_warmed_up() && sensors.gps_parser.last_fix().valid { let (ax_corr, ay_corr, _) = sensors.imu_parser.get_accel_corrected(); @@ -765,21 +785,11 @@ fn main() { estimator.reset_speed(); } + // Update EKF with velocity components (requires full validity) if let Some((vx, vy)) = sensors.gps_parser.get_velocity_enu() { let speed = (vx * vx + vy * vy).sqrt(); estimator.update_velocity(vx, vy); estimator.update_speed(speed); - - // Feed GPS speed to sensor fusion for acceleration calculation - let time_s = now_ms as f32 / 1000.0; - sensor_fusion.process_gps(speed, time_s); - - // Feed GPS heading to yaw rate calibrator (only valid at speed) - // Course over ground is unreliable below ~5 m/s - if speed > 5.0 { - let course = sensors.gps_parser.last_fix().course; - sensor_fusion.process_gps_heading(course); - } } } } diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 138aed6..84960b9 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -26,15 +26,15 @@ pub struct ModeSettings { impl Default for ModeSettings { fn default() -> Self { - // Matches city preset - sensitive defaults for street driving + // Matches city preset - must stay in sync with mode.rs ModeConfig::default() Self { acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) acc_exit: 0.05, // Exit threshold - brake_thr: -0.18, // -0.18g - city driving (normal braking) - brake_exit: -0.09, // Exit threshold - lat_thr: 0.12, // 0.12g - city turns - lat_exit: 0.06, // Exit threshold - yaw_thr: 0.05, // ~2.9°/s yaw rate + brake_thr: -0.15, // -0.15g - sensitive brake detection (compensates for mounting bias) + brake_exit: -0.08, // Exit threshold + lat_thr: 0.10, // 0.10g - sensitive corner detection + lat_exit: 0.05, // Exit threshold + yaw_thr: 0.04, // ~2.3°/s yaw rate min_speed: 2.0, // ~7 km/h } } @@ -299,7 +299,7 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0
Accel0.10/0.05g
-
Brake0.25/0.12g
+
Brake0.15/0.08g
Lateral0.10/0.05g
Yaw0.04 r/s
Min Speed2.0 m/s
@@ -307,8 +307,8 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0
Accel0.10g
Acc Exit0.05g
-
Brake0.25g
-
Brk Exit0.12g
+
Brake0.15g
+
Brk Exit0.08g
Lateral0.10g
Lat Exit0.05g
Yaw0.040r/s
@@ -341,10 +341,10 @@ const CX=70,CY=70,R=55,SCL=R/2; // Canyon: spirited driving (0.20-0.40g range) // Track: racing (0.35-0.80g+ range) const PRESETS={ -track:{acc:0.35,acc_exit:0.17,brake:0.55,brake_exit:0.27,lat:0.50,lat_exit:0.25,yaw:0.15,min_speed:4.0,desc:'Racing/track days'}, -canyon:{acc:0.22,acc_exit:0.11,brake:0.35,brake_exit:0.17,lat:0.28,lat_exit:0.14,yaw:0.10,min_speed:3.0,desc:'Spirited mountain roads'}, -city:{acc:0.10,acc_exit:0.05,brake:0.25,brake_exit:0.12,lat:0.10,lat_exit:0.05,yaw:0.04,min_speed:2.0,desc:'Daily street driving'}, -highway:{acc:0.12,acc_exit:0.06,brake:0.25,brake_exit:0.12,lat:0.12,lat_exit:0.06,yaw:0.04,min_speed:5.0,desc:'Highway cruising'} +track:{acc:0.35,acc_exit:0.17,brake:0.35,brake_exit:0.17,lat:0.50,lat_exit:0.25,yaw:0.15,min_speed:4.0,desc:'Racing/track days'}, +canyon:{acc:0.22,acc_exit:0.11,brake:0.22,brake_exit:0.11,lat:0.28,lat_exit:0.14,yaw:0.10,min_speed:3.0,desc:'Spirited mountain roads'}, +city:{acc:0.10,acc_exit:0.05,brake:0.15,brake_exit:0.08,lat:0.10,lat_exit:0.05,yaw:0.04,min_speed:2.0,desc:'Daily street driving'}, +highway:{acc:0.12,acc_exit:0.06,brake:0.15,brake_exit:0.08,lat:0.12,lat_exit:0.06,yaw:0.04,min_speed:5.0,desc:'Highway cruising'} }; function fmtTime(ms){const s=Math.floor(ms/1000),m=Math.floor(s/60);return String(m).padStart(2,'0')+':'+String(s%60).padStart(2,'0')} From a30b62643dc6d0cb131d1451a5ff454df5640e53 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 12:20:53 -0800 Subject: [PATCH 26/30] formatting --- sensors/blackbox/src/diagnostics.rs | 9 ++++++--- sensors/blackbox/src/main.rs | 28 ++++++++++++++++------------ sensors/blackbox/src/system.rs | 7 ++++--- 3 files changed, 26 insertions(+), 18 deletions(-) diff --git a/sensors/blackbox/src/diagnostics.rs b/sensors/blackbox/src/diagnostics.rs index 89309e8..fdb9963 100644 --- a/sensors/blackbox/src/diagnostics.rs +++ b/sensors/blackbox/src/diagnostics.rs @@ -106,7 +106,8 @@ pub struct FusionDiagnostics { pub lon_blended: f32, // GPS blending status - /// Current GPS blend weight (0.0-1.0, based on orientation correction confidence) + /// Current GPS blend weight (0.0-1.0, based on orientation correction + /// confidence) pub gps_weight: f32, /// GPS-derived acceleration (m/s²), NaN if invalid pub gps_accel: f32, @@ -116,9 +117,11 @@ pub struct FusionDiagnostics { pub gps_rejected: bool, // Orientation corrector (ArduPilot-style GPS-corrected orientation) - /// Learned pitch correction (degrees) - corrects AHRS errors during acceleration + /// Learned pitch correction (degrees) - corrects AHRS errors during + /// acceleration pub pitch_correction_deg: f32, - /// Learned roll correction (degrees) - corrects AHRS errors during cornering + /// Learned roll correction (degrees) - corrects AHRS errors during + /// cornering pub roll_correction_deg: f32, /// Pitch correction confidence (0.0-1.0, based on learning samples) pub pitch_confidence: f32, diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 2cf12fb..9607790 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -18,13 +18,13 @@ use std::sync::Arc; use config::{SystemConfig, WifiModeConfig}; use diagnostics::{DiagnosticsState, FusionDiagnostics}; -use fusion::{FusionConfig, SensorFusion}; use esp_idf_hal::{ delay::FreeRtos, peripherals::Peripherals, uart::{config::Config, UartDriver}, }; use esp_idf_svc::{eventloop::EspSystemEventLoop, nvs::EspDefaultNvsPartition}; +use fusion::{FusionConfig, SensorFusion}; use log::info; use mqtt::MqttClient; use rgb_led::RgbLed; @@ -453,7 +453,8 @@ fn main() { // - IMU orientation is corrected BEFORE gravity removal // - Enables accurate 200 Hz IMU data instead of being limited to GPS rate // - // For longitudinal: Blends corrected IMU with GPS based on correction confidence + // For longitudinal: Blends corrected IMU with GPS based on correction + // confidence // - High confidence → trust IMU more (fast 200 Hz response) // - Low confidence → rely on GPS (ground truth but slower) // @@ -737,8 +738,9 @@ fn main() { } // ALWAYS feed GPS speed to sensor fusion when we have a new fix - // GPS acceleration only needs scalar speed - doesn't require full position validity - // This ensures OrientationCorrector can learn even during brief GPS hiccups + // GPS acceleration only needs scalar speed - doesn't require full position + // validity This ensures OrientationCorrector can learn even during + // brief GPS hiccups if sensors.gps_parser.is_warmed_up() && is_new_fix { let gps_speed = sensors.gps_parser.last_fix().speed; let time_s = now_ms as f32 / 1000.0; @@ -752,7 +754,8 @@ fn main() { } } - // Check if warmup complete (has reference point) AND fix is valid for EKF updates + // Check if warmup complete (has reference point) AND fix is valid for EKF + // updates if sensors.gps_parser.is_warmed_up() && sensors.gps_parser.last_fix().valid { let (ax_corr, ay_corr, _) = sensors.imu_parser.get_accel_corrected(); @@ -818,15 +821,16 @@ fn main() { sensors.is_stationary(ax_corr, ay_corr, sensors.imu_parser.data().wz); // Process through sensor fusion (GPS-corrected orientation + blending) - // Pass raw body-frame IMU data - fusion handles transforms with corrected orientation + // Pass raw body-frame IMU data - fusion handles transforms with corrected + // orientation let dt = config.telemetry.interval_ms as f32 / 1000.0; let (lon_blended, lat_filtered) = sensor_fusion.process_imu( - ax_corr, // Raw body-frame X accel (bias-corrected) - ay_corr, // Raw body-frame Y accel (bias-corrected) - az_corr, // Raw body-frame Z accel (gravity) - sensors.imu_parser.data().roll, // AHRS roll (degrees) - sensors.imu_parser.data().pitch, // AHRS pitch (degrees) - estimator.ekf.yaw(), // EKF yaw (radians) + ax_corr, // Raw body-frame X accel (bias-corrected) + ay_corr, // Raw body-frame Y accel (bias-corrected) + az_corr, // Raw body-frame Z accel (gravity) + sensors.imu_parser.data().roll, // AHRS roll (degrees) + sensors.imu_parser.data().pitch, // AHRS pitch (degrees) + estimator.ekf.yaw(), // EKF yaw (radians) speed, sensors.imu_parser.data().wz, dt, diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index 070dc13..c50c474 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -340,9 +340,10 @@ impl TelemetryPublisher { now_ms: u32, ) -> Result<(), SystemError> { // Get accelerations for display - // lon: GPS-derived when fresh (no vibration), otherwise blended with heavy smoothing - // lat: centripetal (speed × yaw_rate) - instant, doesn't stick after corners - // Uses calibrated yaw rate to prevent drift on highway + // lon: GPS-derived when fresh (no vibration), otherwise blended with heavy + // smoothing lat: centripetal (speed × yaw_rate) - instant, doesn't + // stick after corners Uses calibrated yaw rate to prevent drift on + // highway let lon_display = sensor_fusion.get_lon_display(); let lat_display = sensor_fusion.get_lat_centripetal(); From 84c4f6f0087c5f919c7e6637e1493e2fa81cad1a Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 12:22:36 -0800 Subject: [PATCH 27/30] Fix Python lint errors in analyze_telemetry.py - Rename ambiguous variable 'l' to 'lon'/'true' - Add whitespace around ** operators - Break long lines for readability - Use TRUE_ACCEL_THRESH/TRUE_BRAKE_THRESH constants --- tools/python/analyze_telemetry.py | 51 +++++++++++++++++++------------ 1 file changed, 32 insertions(+), 19 deletions(-) diff --git a/tools/python/analyze_telemetry.py b/tools/python/analyze_telemetry.py index b10157f..a45eb74 100644 --- a/tools/python/analyze_telemetry.py +++ b/tools/python/analyze_telemetry.py @@ -5,7 +5,8 @@ Usage: python analyze_telemetry.py CSV format expected (new format with fusion diagnostics): -time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid,lon_imu,lon_gps,gps_weight,pitch_corr,pitch_conf,roll_corr,roll_conf,tilt_x,tilt_y +time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid, +lon_imu,lon_gps,gps_weight,pitch_corr,pitch_conf,roll_corr,roll_conf,tilt_x,tilt_y Also supports legacy format: time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid @@ -99,7 +100,11 @@ def analyze_telemetry(filename): gps_weights = [float(r['gps_weight']) for r in rows] # Filter out samples where GPS accel is 0 (stale/unavailable) - valid_gps = [(imu, gps, w) for imu, gps, w in zip(lon_imu_vals, lon_gps_vals, gps_weights) if abs(gps) > 0.001] + valid_gps = [ + (imu, gps, w) + for imu, gps, w in zip(lon_imu_vals, lon_gps_vals, gps_weights) + if abs(gps) > 0.001 + ] if valid_gps: imu_valid = [v[0] for v in valid_gps] @@ -111,16 +116,20 @@ def analyze_telemetry(filename): # IMU vs GPS error errors = [i - g for i, g in zip(imu_valid, gps_valid)] mean_error = statistics.mean(errors) + std_error = statistics.stdev(errors) print(f"IMU-GPS error: mean={mean_error:.3f} m/s² ({mean_error/9.81:.3f}g), " - f"std={statistics.stdev(errors):.3f} m/s²") + f"std={std_error:.3f} m/s²") # Correlation between IMU and GPS if len(imu_valid) >= 10: mean_imu = statistics.mean(imu_valid) mean_gps = statistics.mean(gps_valid) - numerator = sum((i - mean_imu) * (g - mean_gps) for i, g in zip(imu_valid, gps_valid)) - denom_imu = sum((i - mean_imu)**2 for i in imu_valid) ** 0.5 - denom_gps = sum((g - mean_gps)**2 for g in gps_valid) ** 0.5 + numerator = sum( + (i - mean_imu) * (g - mean_gps) + for i, g in zip(imu_valid, gps_valid) + ) + denom_imu = sum((i - mean_imu) ** 2 for i in imu_valid) ** 0.5 + denom_gps = sum((g - mean_gps) ** 2 for g in gps_valid) ** 0.5 if denom_imu > 0 and denom_gps > 0: corr = numerator / (denom_imu * denom_gps) print(f"IMU-GPS correlation: {corr:.3f} (want >0.7)") @@ -130,8 +139,9 @@ def analyze_telemetry(filename): high_gps = sum(1 for w in gps_weights if w > 0.7) mid_gps = sum(1 for w in gps_weights if 0.3 <= w <= 0.7) low_gps = sum(1 for w in gps_weights if w < 0.3) - print(f"GPS weight: mean={avg_weight:.2f}, high(>0.7)={100*high_gps/len(rows):.0f}%, " - f"mid={100*mid_gps/len(rows):.0f}%, low(<0.3)={100*low_gps/len(rows):.0f}%") + n = len(rows) + print(f"GPS weight: mean={avg_weight:.2f}, high(>0.7)={100*high_gps/n:.0f}%, " + f"mid={100*mid_gps/n:.0f}%, low(<0.3)={100*low_gps/n:.0f}%") # OrientationCorrector status pitch_corrs = [float(r['pitch_corr']) for r in rows] @@ -209,9 +219,10 @@ def analyze_telemetry(filename): # === MODE ACCURACY ANALYSIS === print("--- MODE DETECTION ACCURACY ---") - # Use lon_g thresholds (City defaults: 0.10g accel, 0.18g brake) - ACCEL_THRESH = 0.10 - BRAKE_THRESH = 0.18 + # Ground truth thresholds for determining "truly accelerating/braking" + # (independent of mode detection thresholds) + TRUE_ACCEL_THRESH = 0.05 # 0.05g from speed changes = real acceleration + TRUE_BRAKE_THRESH = 0.05 # 0.05g from speed changes = real braking tp_accel = fp_accel = fn_accel = 0 tp_brake = fp_brake = fn_brake = 0 @@ -230,8 +241,8 @@ def analyze_telemetry(filename): is_accel_mode = mode in [1, 5] is_brake_mode = mode in [2, 6] - truly_accelerating = true_accel_g > 0.05 - truly_braking = true_accel_g < -0.05 + truly_accelerating = true_accel_g > TRUE_ACCEL_THRESH + truly_braking = true_accel_g < -TRUE_BRAKE_THRESH # ACCEL accuracy if is_accel_mode and truly_accelerating: @@ -282,19 +293,21 @@ def analyze_telemetry(filename): mean_lon = statistics.mean(lon_g_shifted) mean_true = statistics.mean(true_accels) - numerator = sum((l - mean_lon) * (t - mean_true) - for l, t in zip(lon_g_shifted, true_accels)) - denom_lon = sum((l - mean_lon)**2 for l in lon_g_shifted) ** 0.5 - denom_true = sum((t - mean_true)**2 for t in true_accels) ** 0.5 + numerator = sum( + (lon - mean_lon) * (true - mean_true) + for lon, true in zip(lon_g_shifted, true_accels) + ) + denom_lon = sum((lon - mean_lon) ** 2 for lon in lon_g_shifted) ** 0.5 + denom_true = sum((t - mean_true) ** 2 for t in true_accels) ** 0.5 if denom_lon > 0 and denom_true > 0: correlation = numerator / (denom_lon * denom_true) print(f"Correlation coefficient: {correlation:.3f} (want >0.7)") # Calculate error - errors = [l - t for l, t in zip(lon_g_shifted, true_accels)] + errors = [lon - t for lon, t in zip(lon_g_shifted, true_accels)] mean_error = statistics.mean(errors) - rmse = (sum(e**2 for e in errors) / len(errors)) ** 0.5 + rmse = (sum(e ** 2 for e in errors) / len(errors)) ** 0.5 print(f"Mean error (lon_g - true): {mean_error:.4f}g (bias)") print(f"RMSE: {rmse:.4f}g") print() From 1375792588bf77b23269163be417f2d27666efbd Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 12:26:25 -0800 Subject: [PATCH 28/30] Fix remaining Python lint errors - Add whitespace around * operators in f-strings - Remove unused lon_g variable - Break long lines (<120 chars) --- tools/python/analyze_telemetry.py | 41 +++++++++++++++++-------------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/tools/python/analyze_telemetry.py b/tools/python/analyze_telemetry.py index a45eb74..95bb5e1 100644 --- a/tools/python/analyze_telemetry.py +++ b/tools/python/analyze_telemetry.py @@ -58,10 +58,11 @@ def analyze_telemetry(filename): slow = sum(1 for s in speeds if 2 <= s < 20) medium = sum(1 for s in speeds if 20 <= s < 50) fast = sum(1 for s in speeds if s >= 50) - print(f"Time distribution: stopped(<2)={100*stopped/len(rows):.0f}%, " - f"slow(2-20)={100*slow/len(rows):.0f}%, " - f"medium(20-50)={100*medium/len(rows):.0f}%, " - f"fast(>50)={100*fast/len(rows):.0f}%") + n = len(rows) + print(f"Time distribution: stopped(<2)={100 * stopped / n:.0f}%, " + f"slow(2-20)={100 * slow / n:.0f}%, " + f"medium(20-50)={100 * medium / n:.0f}%, " + f"fast(>50)={100 * fast / n:.0f}%") print() # === ACCELERATION ANALYSIS === @@ -140,8 +141,8 @@ def analyze_telemetry(filename): mid_gps = sum(1 for w in gps_weights if 0.3 <= w <= 0.7) low_gps = sum(1 for w in gps_weights if w < 0.3) n = len(rows) - print(f"GPS weight: mean={avg_weight:.2f}, high(>0.7)={100*high_gps/n:.0f}%, " - f"mid={100*mid_gps/n:.0f}%, low(<0.3)={100*low_gps/n:.0f}%") + print(f"GPS weight: mean={avg_weight:.2f}, high(>0.7)={100 * high_gps / n:.0f}%, " + f"mid={100 * mid_gps / n:.0f}%, low(<0.3)={100 * low_gps / n:.0f}%") # OrientationCorrector status pitch_corrs = [float(r['pitch_corr']) for r in rows] @@ -149,10 +150,14 @@ def analyze_telemetry(filename): roll_corrs = [float(r['roll_corr']) for r in rows] roll_confs = [float(r['roll_conf']) for r in rows] - print(f"Pitch correction: {statistics.mean(pitch_corrs):.1f}° (range {min(pitch_corrs):.1f}° to {max(pitch_corrs):.1f}°)") - print(f"Pitch confidence: {statistics.mean(pitch_confs):.0f}% (range {min(pitch_confs):.0f}% to {max(pitch_confs):.0f}%)") - print(f"Roll correction: {statistics.mean(roll_corrs):.1f}° (range {min(roll_corrs):.1f}° to {max(roll_corrs):.1f}°)") - print(f"Roll confidence: {statistics.mean(roll_confs):.0f}% (range {min(roll_confs):.0f}% to {max(roll_confs):.0f}%)") + pc_mean, pc_min, pc_max = statistics.mean(pitch_corrs), min(pitch_corrs), max(pitch_corrs) + print(f"Pitch correction: {pc_mean:.1f}° (range {pc_min:.1f}° to {pc_max:.1f}°)") + pcf_mean, pcf_min, pcf_max = statistics.mean(pitch_confs), min(pitch_confs), max(pitch_confs) + print(f"Pitch confidence: {pcf_mean:.0f}% (range {pcf_min:.0f}% to {pcf_max:.0f}%)") + rc_mean, rc_min, rc_max = statistics.mean(roll_corrs), min(roll_corrs), max(roll_corrs) + print(f"Roll correction: {rc_mean:.1f}° (range {rc_min:.1f}° to {rc_max:.1f}°)") + rcf_mean, rcf_min, rcf_max = statistics.mean(roll_confs), min(roll_confs), max(roll_confs) + print(f"Roll confidence: {rcf_mean:.0f}% (range {rcf_min:.0f}% to {rcf_max:.0f}%)") # Tilt offsets tilt_x = [float(r['tilt_x']) for r in rows] @@ -183,14 +188,15 @@ def analyze_telemetry(filename): modes = Counter(int(float(r['mode'])) for r in rows) mode_names = {0: 'IDLE', 1: 'ACCEL', 2: 'BRAKE', 4: 'CORNER', 5: 'ACCEL+CORNER', 6: 'BRAKE+CORNER'} + total = len(rows) for mode, count in sorted(modes.items()): name = mode_names.get(mode, f'UNKNOWN({mode})') - print(f" {name:15} {count:5} ({100*count/len(rows):5.1f}%)") + print(f" {name:15} {count:5} ({100 * count / total:5.1f}%)") total_accel = modes.get(1, 0) + modes.get(5, 0) total_brake = modes.get(2, 0) + modes.get(6, 0) - print(f"\n Total ACCEL modes: {total_accel} ({100*total_accel/len(rows):.1f}%)") - print(f" Total BRAKE modes: {total_brake} ({100*total_brake/len(rows):.1f}%)") + print(f"\n Total ACCEL modes: {total_accel} ({100 * total_accel / total:.1f}%)") + print(f" Total BRAKE modes: {total_brake} ({100 * total_brake / total:.1f}%)") print(f" Ratio ACCEL/BRAKE: {total_accel/max(total_brake,1):.1f}x (should be ~1x for balanced driving)") print() @@ -229,7 +235,6 @@ def analyze_telemetry(filename): for i in range(1, len(rows)): mode = int(float(rows[i]['mode'])) - lon_g = float(rows[i]['lon_g']) # Use speed-derived acceleration as ground truth dt = (int(rows[i]['time']) - int(rows[i-1]['time'])) / 1000.0 @@ -266,10 +271,10 @@ def analyze_telemetry(filename): print(f" False Negatives: {fn_accel:5} (accelerating but mode!=ACCEL)") if tp_accel + fp_accel > 0: precision = tp_accel / (tp_accel + fp_accel) - print(f" Precision: {100*precision:.1f}% (want >80%)") + print(f" Precision: {100 * precision:.1f}% (want >80%)") if tp_accel + fn_accel > 0: recall = tp_accel / (tp_accel + fn_accel) - print(f" Recall: {100*recall:.1f}%") + print(f" Recall: {100 * recall:.1f}%") print(f"\nBRAKE detection:") print(f" True Positives: {tp_brake:5}") @@ -277,10 +282,10 @@ def analyze_telemetry(filename): print(f" False Negatives: {fn_brake:5} (braking but mode!=BRAKE)") if tp_brake + fp_brake > 0: precision = tp_brake / (tp_brake + fp_brake) - print(f" Precision: {100*precision:.1f}% (want >80%)") + print(f" Precision: {100 * precision:.1f}% (want >80%)") if tp_brake + fn_brake > 0: recall = tp_brake / (tp_brake + fn_brake) - print(f" Recall: {100*recall:.1f}%") + print(f" Recall: {100 * recall:.1f}%") print() # === lon_g vs TRUE ACCEL CORRELATION === From 1382b1162511491a19bceb1bb654b71bc2c7de70 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 12:27:01 -0800 Subject: [PATCH 29/30] Apply cargo fmt formatting --- framework/examples/drive_sim.rs | 129 +++++++++++----- framework/src/fusion.rs | 265 ++++++++++++++++---------------- framework/src/mode.rs | 8 +- 3 files changed, 231 insertions(+), 171 deletions(-) diff --git a/framework/examples/drive_sim.rs b/framework/examples/drive_sim.rs index 52c4286..dce08d4 100644 --- a/framework/examples/drive_sim.rs +++ b/framework/examples/drive_sim.rs @@ -53,8 +53,16 @@ fn main() { let az_noise = noise.next(0.02 * G); fusion.process_imu( - ax_noise, ay_noise, G + az_noise, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.005, true + ax_noise, + ay_noise, + G + az_noise, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.005, + true, ); if i % 200 == 199 { println!(" ... {}s", (i + 1) / 200); @@ -62,8 +70,14 @@ fn main() { } let (tilt_x, tilt_y, tilt_valid) = fusion.get_tilt_offsets(); let (pitch_corr, roll_corr) = fusion.get_orientation_correction(); - println!(" Tilt learned: x={:.3}, y={:.3}, valid={}", tilt_x, tilt_y, tilt_valid); - println!(" Orientation correction: pitch={:.1}°, roll={:.1}°\n", pitch_corr, roll_corr); + println!( + " Tilt learned: x={:.3}, y={:.3}, valid={}", + tilt_x, tilt_y, tilt_valid + ); + println!( + " Orientation correction: pitch={:.1}°, roll={:.1}°\n", + pitch_corr, roll_corr + ); // Phase 2: Acceleration (2 seconds, 0.3g) // CRITICAL: Simulate WT901 AHRS error - it reports false pitch during acceleration! @@ -90,11 +104,16 @@ fn main() { let ay_noise = noise.next(0.05 * G); let (lon, lat) = fusion.process_imu( - accel + ax_noise, ay_noise, G, // Body-frame: forward accel + gravity - 0.0, false_pitch, // AHRS reports false pitch! + accel + ax_noise, + ay_noise, + G, // Body-frame: forward accel + gravity + 0.0, + false_pitch, // AHRS reports false pitch! + 0.0, + speed, 0.0, - speed, 0.0, - 0.005, false + 0.005, + false, ); mode_classifier.update_hybrid(lon, lat, 0.0, speed); let mode = mode_classifier.get_mode(); @@ -127,16 +146,22 @@ fn main() { let ay_noise = noise.next(0.05 * G); let (lon, lat) = fusion.process_imu( - ax_noise, ay_noise, G, - 0.0, 0.0, // AHRS reports level during cruise + ax_noise, + ay_noise, + G, 0.0, - cruise_speed, 0.0, - 0.005, false + 0.0, // AHRS reports level during cruise + 0.0, + cruise_speed, + 0.0, + 0.005, + false, ); mode_classifier.update_hybrid(lon, lat, 0.0, cruise_speed); let mode = mode_classifier.get_mode(); - if i > 200 { // After filter settles + if i > 200 { + // After filter settles max_lon = max_lon.max(lon.abs()); if mode.has_accel() { accel_count += 1; @@ -150,13 +175,22 @@ fn main() { if i % 200 == 199 { let (pitch_corr, _) = fusion.get_orientation_correction(); let (pitch_conf, _) = fusion.get_orientation_confidence(); - println!(" t={:.1}s: lon={:.3} m/s², mode={}, pitch_corr={:.1}°, conf={:.0}%", - (i + 1) as f32 * 0.005, lon, mode_str(&mode), pitch_corr, pitch_conf * 100.0); + println!( + " t={:.1}s: lon={:.3} m/s², mode={}, pitch_corr={:.1}°, conf={:.0}%", + (i + 1) as f32 * 0.005, + lon, + mode_str(&mode), + pitch_corr, + pitch_conf * 100.0 + ); } } println!("\n CRUISE RESULTS:"); println!(" Max |lon|: {:.3} m/s² (should be < 0.5)", max_lon); - println!(" Mode counts: IDLE={}, ACCEL={}, BRAKE={}", idle_count, accel_count, brake_count); + println!( + " Mode counts: IDLE={}, ACCEL={}, BRAKE={}", + idle_count, accel_count, brake_count + ); if brake_count > 0 || accel_count > 0 { println!(" ⚠️ WARNING: False mode detections during cruise!"); } else { @@ -186,11 +220,16 @@ fn main() { let ay_noise = noise.next(0.05 * G); let (lon, lat) = fusion.process_imu( - ax_noise, lat_accel + ay_noise, G, // Lateral accel in body Y - false_roll, 0.0, // AHRS reports false roll + ax_noise, + lat_accel + ay_noise, + G, // Lateral accel in body Y + false_roll, + 0.0, // AHRS reports false roll yaw, - cruise_speed, turn_yaw_rate, - 0.005, false + cruise_speed, + turn_yaw_rate, + 0.005, + false, ); mode_classifier.update_hybrid(lon, lat, turn_yaw_rate, cruise_speed); let mode = mode_classifier.get_mode(); @@ -206,7 +245,10 @@ fn main() { (i + 1) as f32 * 0.005, lat, mode_str(&mode), false_roll, roll_corr, roll_conf * 100.0); } } - println!(" CORNER detection rate: {:.0}%\n", 100.0 * corner_count as f32 / 600.0); + println!( + " CORNER detection rate: {:.0}%\n", + 100.0 * corner_count as f32 / 600.0 + ); // Phase 5: Braking (2 seconds, -0.4g) println!("Phase 5: BRAKING (2s at -0.4g = -3.92 m/s²)"); @@ -225,18 +267,29 @@ fn main() { let ay_noise = noise.next(0.05 * G); let (lon, lat) = fusion.process_imu( - accel + ax_noise, ay_noise, G, - 0.0, false_pitch, + accel + ax_noise, + ay_noise, + G, + 0.0, + false_pitch, yaw, - speed, 0.0, - 0.005, false + speed, + 0.0, + 0.005, + false, ); mode_classifier.update_hybrid(lon, lat, 0.0, speed); let mode = mode_classifier.get_mode(); if i % 100 == 99 { - println!(" t={:.1}s: speed={:.1} m/s, lon={:.2} m/s², mode={}, AHRS_pitch={:.1}°", - (i + 1) as f32 * 0.005, speed, lon, mode_str(&mode), false_pitch); + println!( + " t={:.1}s: speed={:.1} m/s, lon={:.2} m/s², mode={}, AHRS_pitch={:.1}°", + (i + 1) as f32 * 0.005, + speed, + lon, + mode_str(&mode), + false_pitch + ); } } println!(); @@ -247,13 +300,8 @@ fn main() { let ax_noise = noise.next(0.02 * G); let ay_noise = noise.next(0.02 * G); - let (lon, lat) = fusion.process_imu( - ax_noise, ay_noise, G, - 0.0, 0.0, - yaw, - 0.0, 0.0, - 0.005, true - ); + let (lon, lat) = + fusion.process_imu(ax_noise, ay_noise, G, 0.0, 0.0, yaw, 0.0, 0.0, 0.005, true); mode_classifier.update_hybrid(lon, lat, 0.0, 0.0); let mode = mode_classifier.get_mode(); @@ -261,14 +309,21 @@ fn main() { let (pitch_corr, roll_corr) = fusion.get_orientation_correction(); let (pitch_conf, roll_conf) = fusion.get_orientation_confidence(); println!(" Final: lon={:.3} m/s², mode={}", lon, mode_str(&mode)); - println!(" Final orientation correction: pitch={:.1}° ({:.0}%), roll={:.1}° ({:.0}%)", - pitch_corr, pitch_conf * 100.0, roll_corr, roll_conf * 100.0); + println!( + " Final orientation correction: pitch={:.1}° ({:.0}%), roll={:.1}° ({:.0}%)", + pitch_corr, + pitch_conf * 100.0, + roll_corr, + roll_conf * 100.0 + ); } } println!("\n=== Simulation Complete ==="); println!("\nSummary:"); - println!("- WT901 AHRS error was simulated: false pitch during accel, false roll during cornering"); + println!( + "- WT901 AHRS error was simulated: false pitch during accel, false roll during cornering" + ); println!("- OrientationCorrector learned to compensate for these errors"); println!("- Corrected IMU provides accurate 200 Hz acceleration despite AHRS errors"); println!("- GPS is used for ground truth during learning and as confidence-based fallback"); diff --git a/framework/src/fusion.rs b/framework/src/fusion.rs index 07ff104..f110dc4 100644 --- a/framework/src/fusion.rs +++ b/framework/src/fusion.rs @@ -95,10 +95,10 @@ impl OrientationCorrector { cruise_bias_sum: 0.0, cruise_bias_count: 0, cruise_time: 0.0, - alpha: 0.05, // Slow learning for stability - min_speed: 3.0, // ~11 km/h - min_accel: 0.3, // ~0.03g - lowered to learn from more events - max_correction: 0.26, // ~15 degrees max + alpha: 0.05, // Slow learning for stability + min_speed: 3.0, // ~11 km/h + min_accel: 0.3, // ~0.03g - lowered to learn from more events + max_correction: 0.26, // ~15 degrees max update_count: 0, } } @@ -143,9 +143,11 @@ impl OrientationCorrector { let pitch_innovation = lon_error / G; // EMA update with clamping - self.pitch_correction = (1.0 - self.alpha) * self.pitch_correction - + self.alpha * pitch_innovation; - self.pitch_correction = self.pitch_correction.clamp(-self.max_correction, self.max_correction); + self.pitch_correction = + (1.0 - self.alpha) * self.pitch_correction + self.alpha * pitch_innovation; + self.pitch_correction = self + .pitch_correction + .clamp(-self.max_correction, self.max_correction); // Update confidence self.update_count = self.update_count.saturating_add(1); @@ -184,9 +186,11 @@ impl OrientationCorrector { if lat_gps.abs() > self.min_accel { let roll_innovation = lat_error / G; - self.roll_correction = (1.0 - self.alpha) * self.roll_correction - + self.alpha * roll_innovation; - self.roll_correction = self.roll_correction.clamp(-self.max_correction, self.max_correction); + self.roll_correction = + (1.0 - self.alpha) * self.roll_correction + self.alpha * roll_innovation; + self.roll_correction = self + .roll_correction + .clamp(-self.max_correction, self.max_correction); self.roll_confidence = (self.update_count as f32 / 100.0).min(1.0); } @@ -205,7 +209,10 @@ impl OrientationCorrector { let pitch_corr_deg = self.pitch_correction.to_degrees(); let roll_corr_deg = self.roll_correction.to_degrees(); - (ahrs_pitch_deg - pitch_corr_deg, ahrs_roll_deg - roll_corr_deg) + ( + ahrs_pitch_deg - pitch_corr_deg, + ahrs_roll_deg - roll_corr_deg, + ) } /// Get current pitch correction in degrees (for diagnostics) @@ -558,7 +565,7 @@ pub struct YawRateCalibrator { yaw_sum: f32, sample_count: u32, /// Configuration - min_speed: f32, // m/s, minimum speed for calibration (~29 km/h) + min_speed: f32, // m/s, minimum speed for calibration (~29 km/h) heading_tolerance: f32, // radians, max heading deviation for "straight" (~2°) learn_time: f32, // seconds, time required to update bias alpha: f32, // EMA alpha for bias update @@ -799,16 +806,28 @@ impl SensorFusion { // Step 1: Apply orientation correction (learned from GPS comparison) // This corrects AHRS errors caused by linear acceleration being mistaken for tilt - let (corrected_pitch_deg, corrected_roll_deg) = - self.orientation_corrector.correct(ahrs_pitch_deg, ahrs_roll_deg); + let (corrected_pitch_deg, corrected_roll_deg) = self + .orientation_corrector + .correct(ahrs_pitch_deg, ahrs_roll_deg); // Step 2: Remove gravity using corrected orientation - let (ax_nograv, ay_nograv, _az_nograv) = - remove_gravity(ax_raw, ay_raw, az_raw, corrected_roll_deg, corrected_pitch_deg); + let (ax_nograv, ay_nograv, _az_nograv) = remove_gravity( + ax_raw, + ay_raw, + az_raw, + corrected_roll_deg, + corrected_pitch_deg, + ); // Step 3: Transform to earth frame - let (ax_earth, ay_earth) = - body_to_earth(ax_nograv, ay_nograv, 0.0, corrected_roll_deg, corrected_pitch_deg, yaw); + let (ax_earth, ay_earth) = body_to_earth( + ax_nograv, + ay_nograv, + 0.0, + corrected_roll_deg, + corrected_pitch_deg, + yaw, + ); // Step 4: Apply residual tilt correction (learned when stopped) // This handles minor mounting offsets not captured by orientation correction @@ -837,7 +856,8 @@ impl SensorFusion { // Step 9: Handle stationary state - learn residual offsets if is_stationary { - self.tilt_estimator.update_stationary(ax_earth, ay_earth, dt); + self.tilt_estimator + .update_stationary(ax_earth, ay_earth, dt); } else { if self.was_stationary { self.tilt_estimator.reset_stationary(); @@ -861,7 +881,7 @@ impl SensorFusion { gps_lat, // Centripetal (truth for lateral) speed, gps_fresh, - dt, // Time step for cruise bias learning + dt, // Time step for cruise bias learning ); } } @@ -1151,11 +1171,7 @@ mod tests { // During turn, lateral should be positive (speed * positive yaw_rate) if i > 5 { - assert!( - lat > 10.0, - "During turn, lateral should be high: {}", - lat - ); + assert!(lat > 10.0, "During turn, lateral should be high: {}", lat); } } @@ -1194,8 +1210,8 @@ mod tests { let (lon_from_process, _lat) = fusion.process_imu( 5.0, 3.0, G, // raw body accel (forward, lateral, gravity) - 0.0, 0.0, // roll, pitch (level) - 0.0, 10.0, 0.0, 0.05, false + 0.0, 0.0, // roll, pitch (level) + 0.0, 10.0, 0.0, 0.05, false, ); let lon_for_display = fusion.get_lon_blended(); @@ -1234,7 +1250,11 @@ mod tests { // After 1 second with 25 fixes gps.update_rate(25, 2.0); // First update, EMA: 0.3 * 25 + 0.7 * 0 = 7.5 - assert!((gps.get_rate() - 7.5).abs() < 1.0, "First rate should be ~7.5: got {}", gps.get_rate()); + assert!( + (gps.get_rate() - 7.5).abs() < 1.0, + "First rate should be ~7.5: got {}", + gps.get_rate() + ); // After another second with 25 more fixes gps.update_rate(50, 3.0); @@ -1262,7 +1282,10 @@ mod tests { } // Calibrator should have learned the bias - assert!(cal.is_valid(), "Should have valid calibration after 4s straight"); + assert!( + cal.is_valid(), + "Should have valid calibration after 4s straight" + ); assert!( (cal.get_bias() - bias).abs() < 0.005, "Learned bias should be close to actual: {} vs {}", @@ -1294,10 +1317,7 @@ mod tests { } // Should NOT have valid calibration because heading wasn't stable - assert!( - !cal.is_valid(), - "Should reject calibration during turns" - ); + assert!(!cal.is_valid(), "Should reject calibration during turns"); } #[test] @@ -1321,7 +1341,8 @@ mod tests { // Now with calibration active, centripetal should be near zero // even though raw yaw_rate has bias - let (_lon, lat) = fusion.process_imu(0.0, 0.0, G, 0.0, 0.0, 0.0, 20.0, yaw_bias, 0.05, false); + let (_lon, lat) = + fusion.process_imu(0.0, 0.0, G, 0.0, 0.0, 0.0, 20.0, yaw_bias, 0.05, false); // Without calibration: lat = 20 * 0.02 = 0.4 m/s² // With calibration: lat should be ~0 @@ -1340,9 +1361,9 @@ mod tests { let config = FusionConfig::default(); let mut fusion = SensorFusion::new(config); - let noise_freq = 12.0; // Hz - above 10Hz cutoff, below 15Hz Nyquist - let noise_amp = 1.0; // m/s² (~0.1g) - let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) + let noise_freq = 12.0; // Hz - above 10Hz cutoff, below 15Hz Nyquist + let noise_amp = 1.0; // m/s² (~0.1g) + let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) // Run for 3 seconds to let filter settle let mut max_output: f32 = 0.0; @@ -1352,16 +1373,16 @@ mod tests { let noise = noise_amp * (2.0 * core::f32::consts::PI * noise_freq * t).sin(); let (lon, _lat) = fusion.process_imu( - noise, // ax_raw = noisy - 0.0, // ay_raw = 0 - G, // az_raw = gravity - 0.0, // roll = level - 0.0, // pitch = level - 0.0, // yaw = 0 (heading east) - 10.0, // speed - 0.0, // yaw_rate = 0 - 0.033, // dt = 33ms (~30Hz) - false, // not stationary + noise, // ax_raw = noisy + 0.0, // ay_raw = 0 + G, // az_raw = gravity + 0.0, // roll = level + 0.0, // pitch = level + 0.0, // yaw = 0 (heading east) + 10.0, // speed + 0.0, // yaw_rate = 0 + 0.033, // dt = 33ms (~30Hz) + false, // not stationary ); // After settling (1 second = 30 samples), check output @@ -1387,9 +1408,9 @@ mod tests { let config = FusionConfig::default(); let mut fusion = SensorFusion::new(config); - let dynamics_freq = 1.0; // Hz - braking event - let dynamics_amp = 3.0; // m/s² (~0.3g braking) - let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) + let dynamics_freq = 1.0; // Hz - braking event + let dynamics_amp = 3.0; // m/s² (~0.3g braking) + let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) // Run for 5 seconds to let filter settle let mut max_output: f32 = 0.0; @@ -1464,8 +1485,8 @@ mod tests { // Process IMU with non-zero input let (lon_blended, _lat) = fusion.process_imu( 2.0, 0.0, G, // ax, ay, az (body frame) - 0.0, 0.0, // roll, pitch - 0.0, 10.0, 0.0, 0.005, false + 0.0, 0.0, // roll, pitch + 0.0, 10.0, 0.0, 0.005, false, ); let lon_display = fusion.get_lon_display(); @@ -1552,14 +1573,14 @@ mod tests { for i in 0..200 { let (l, _lat) = fusion.process_imu( imu_fake_accel, // Fake signal from AHRS pitch error - 0.0, // ay_raw - G, // az_raw - 0.0, // roll - 0.0, // pitch (AHRS reports 0, but there's error we're simulating) - 0.0, // yaw - 10.0, // speed - 0.0, // yaw_rate - 0.005, // dt + 0.0, // ay_raw + G, // az_raw + 0.0, // roll + 0.0, // pitch (AHRS reports 0, but there's error we're simulating) + 0.0, // yaw + 10.0, // speed + 0.0, // yaw_rate + 0.005, // dt false, ); // Keep GPS fresh @@ -1595,12 +1616,12 @@ mod tests { // 3 seconds at 200Hz fusion.process_imu( 0.0, 0.0, G, // ax, ay, az (level, no motion) - 0.0, 0.0, // roll, pitch - 0.0, // yaw - 0.0, // Stopped - 0.0, // No yaw rate - 0.005, // dt = 5ms - true, // STATIONARY - tilt learns here + 0.0, 0.0, // roll, pitch + 0.0, // yaw + 0.0, // Stopped + 0.0, // No yaw rate + 0.005, // dt = 5ms + true, // STATIONARY - tilt learns here ); } @@ -1619,12 +1640,12 @@ mod tests { let (lon, _lat) = fusion.process_imu( 0.0, 0.0, G, // ax, ay, az - ZERO accel! - 0.0, 0.0, // roll, pitch - 0.0, // yaw - 20.0, // 20 m/s = 72 km/h cruising - 0.0, // Straight road - 0.005, // dt = 5ms - false, // Moving + 0.0, 0.0, // roll, pitch + 0.0, // yaw + 20.0, // 20 m/s = 72 km/h cruising + 0.0, // Straight road + 0.005, // dt = 5ms + false, // Moving ); // Skip filter settling time @@ -1699,11 +1720,11 @@ mod tests { let (lon, _) = fusion.process_imu( 0.0, 0.0, G, // ax, ay, az - zero input - 0.0, 0.0, // roll, pitch - 0.0, // yaw - 25.0, // 25 m/s = 90 km/h - 0.0, // yaw_rate - 0.005, // dt + 0.0, 0.0, // roll, pitch + 0.0, // yaw + 25.0, // 25 m/s = 90 km/h + 0.0, // yaw_rate + 0.005, // dt false, ); @@ -1756,20 +1777,19 @@ mod tests { let mut corrector = OrientationCorrector::new(); - let lon_gps = 3.0; // Ground truth + let lon_gps = 3.0; // Ground truth let lon_imu = 2.49; // What IMU shows with -3° pitch error - let speed = 15.0; // 54 km/h, above min_speed + let speed = 15.0; // 54 km/h, above min_speed // Simulate many updates (EMA needs time to converge) for _ in 0..500 { corrector.update( - lon_imu, // IMU longitudinal - lon_gps, // GPS longitudinal (truth) - 0.0, // lat_imu - 0.0, // lat_gps - speed, - true, // GPS fresh - 0.033, // dt (~30Hz) + lon_imu, // IMU longitudinal + lon_gps, // GPS longitudinal (truth) + 0.0, // lat_imu + 0.0, // lat_gps + speed, true, // GPS fresh + 0.033, // dt (~30Hz) ); } @@ -1797,19 +1817,17 @@ mod tests { // If AHRS has roll error, lat_imu differs from lat_gps let mut corrector = OrientationCorrector::new(); - let lat_gps = 5.0; // Centripetal = speed * yaw_rate = 10 * 0.5 = 5 m/s² - let lat_imu = 4.5; // Roll error causes 0.5 m/s² difference + let lat_gps = 5.0; // Centripetal = speed * yaw_rate = 10 * 0.5 = 5 m/s² + let lat_imu = 4.5; // Roll error causes 0.5 m/s² difference let speed = 15.0; for _ in 0..500 { corrector.update( - 0.0, // lon_imu (not accelerating) - 0.0, // lon_gps - lat_imu, // IMU lateral - lat_gps, // GPS lateral (centripetal) - speed, - true, - 0.005, // dt = 5ms (200Hz) + 0.0, // lon_imu (not accelerating) + 0.0, // lon_gps + lat_imu, // IMU lateral + lat_gps, // GPS lateral (centripetal) + speed, true, 0.005, // dt = 5ms (200Hz) ); } @@ -1832,13 +1850,9 @@ mod tests { for _ in 0..100 { corrector.update( - 5.0, // Large IMU value - 0.0, // GPS shows zero - 0.0, - 0.0, - low_speed, - true, - 0.005, // dt = 5ms + 5.0, // Large IMU value + 0.0, // GPS shows zero + 0.0, 0.0, low_speed, true, 0.005, // dt = 5ms ); } @@ -1861,13 +1875,10 @@ mod tests { // BUT now this should learn cruise_bias instead! for _ in 0..100 { corrector.update( - 0.3, // Small IMU value (offset) - 0.0, // GPS shows zero (cruising) - 0.0, - 0.0, - 15.0, // Good speed - true, - 0.05, // dt = 50ms (shorter loop, 2s = 40 iterations, we do 100) + 0.3, // Small IMU value (offset) + 0.0, // GPS shows zero (cruising) + 0.0, 0.0, 15.0, // Good speed + true, 0.05, // dt = 50ms (shorter loop, 2s = 40 iterations, we do 100) ); } @@ -1892,7 +1903,7 @@ mod tests { // Huge error that would exceed max_correction let lon_imu = 10.0; - let lon_gps = 3.0; // 7 m/s² difference = huge pitch error + let lon_gps = 3.0; // 7 m/s² difference = huge pitch error for _ in 0..1000 { corrector.update(lon_imu, lon_gps, 0.0, 0.0, 15.0, true, 0.005); @@ -1912,8 +1923,8 @@ mod tests { let mut corrector = OrientationCorrector::new(); // Manually set corrections for testing - corrector.pitch_correction = 0.05; // ~2.9° - corrector.roll_correction = -0.03; // ~-1.7° + corrector.pitch_correction = 0.05; // ~2.9° + corrector.roll_correction = -0.03; // ~-1.7° // AHRS reports pitch=5°, roll=2° let (corrected_pitch, corrected_roll) = corrector.correct(5.0, 2.0); @@ -1960,12 +1971,7 @@ mod tests { // GPS not fresh - should not learn for _ in 0..100 { corrector.update( - 5.0, - 3.0, - 0.0, - 0.0, - 15.0, - false, // GPS NOT fresh + 5.0, 3.0, 0.0, 0.0, 15.0, false, // GPS NOT fresh 0.005, ); } @@ -1985,18 +1991,15 @@ mod tests { // Simulate 5 seconds of cruising at constant speed // IMU shows 0.5 m/s² offset (mounting error), GPS shows ~0 (cruising) let imu_offset = 0.5; // m/s² - consistent offset during cruise - let dt = 0.05; // 50ms = 20Hz, so 2s = 40 samples + let dt = 0.05; // 50ms = 20Hz, so 2s = 40 samples // Run for 5 seconds (100 iterations at 50ms each) for _ in 0..100 { corrector.update( - imu_offset, // IMU shows offset - 0.0, // GPS shows zero (cruising) - 0.0, - 0.0, - 15.0, // 54 km/h - above min_speed - true, - dt, + imu_offset, // IMU shows offset + 0.0, // GPS shows zero (cruising) + 0.0, 0.0, 15.0, // 54 km/h - above min_speed + true, dt, ); } @@ -2039,13 +2042,9 @@ mod tests { for _ in 0..10 { corrector.update( - 3.5, // IMU shows acceleration - 3.0, // GPS also shows acceleration - 0.0, - 0.0, - 15.0, - true, - 0.05, + 3.5, // IMU shows acceleration + 3.0, // GPS also shows acceleration + 0.0, 0.0, 15.0, true, 0.05, ); } diff --git a/framework/src/mode.rs b/framework/src/mode.rs index 00e199f..ee8ae9e 100644 --- a/framework/src/mode.rs +++ b/framework/src/mode.rs @@ -309,7 +309,13 @@ impl ModeClassifier { /// * `a_lat_centripetal` - Centripetal lateral acceleration (m/s², = speed × yaw_rate) /// * `wz` - Yaw rate (rad/s) /// * `speed` - Vehicle speed (m/s) - pub fn update_hybrid(&mut self, a_lon_blended: f32, a_lat_centripetal: f32, wz: f32, speed: f32) { + pub fn update_hybrid( + &mut self, + a_lon_blended: f32, + a_lat_centripetal: f32, + wz: f32, + speed: f32, + ) { // Update display speed with EMA self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; if self.v_disp < 3.0 / 3.6 { From ef8f3bafbc6fc1101e58b16e6fe3f2e29cccf183 Mon Sep 17 00:00:00 2001 From: Jose Cruz Toledo <1273555+jctoledo@users.noreply.github.com> Date: Mon, 19 Jan 2026 12:27:58 -0800 Subject: [PATCH 30/30] Fix E226: add whitespace around - in array indices --- tools/python/analyze_telemetry.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tools/python/analyze_telemetry.py b/tools/python/analyze_telemetry.py index 95bb5e1..4bec068 100644 --- a/tools/python/analyze_telemetry.py +++ b/tools/python/analyze_telemetry.py @@ -39,7 +39,7 @@ def analyze_telemetry(filename): # === TIMING ANALYSIS === print("--- TIMING ---") times = [int(r['time']) for r in rows] - intervals = [times[i] - times[i-1] for i in range(1, len(times))] + intervals = [times[i] - times[i - 1] for i in range(1, len(times))] if intervals: avg_interval = statistics.mean(intervals) std_interval = statistics.stdev(intervals) if len(intervals) > 1 else 0 @@ -83,7 +83,7 @@ def analyze_telemetry(filename): # Check for bias - compare when speed is stable stable_lon = [] for i in range(1, len(rows)): - speed_diff = abs(float(rows[i]['speed']) - float(rows[i-1]['speed'])) + speed_diff = abs(float(rows[i]['speed']) - float(rows[i - 1]['speed'])) if speed_diff < 0.3: # Speed stable within 0.3 km/h stable_lon.append(float(rows[i]['lon_g'])) if stable_lon: @@ -206,9 +206,9 @@ def analyze_telemetry(filename): # Calculate acceleration from speed changes true_accels = [] for i in range(1, len(rows)): - dt = (int(rows[i]['time']) - int(rows[i-1]['time'])) / 1000.0 # seconds + dt = (int(rows[i]['time']) - int(rows[i - 1]['time'])) / 1000.0 # seconds if dt > 0: - dv = (float(rows[i]['speed']) - float(rows[i-1]['speed'])) / 3.6 # m/s + dv = (float(rows[i]['speed']) - float(rows[i - 1]['speed'])) / 3.6 # m/s accel_g = (dv / dt) / 9.80665 # in G true_accels.append(accel_g) @@ -237,9 +237,9 @@ def analyze_telemetry(filename): mode = int(float(rows[i]['mode'])) # Use speed-derived acceleration as ground truth - dt = (int(rows[i]['time']) - int(rows[i-1]['time'])) / 1000.0 + dt = (int(rows[i]['time']) - int(rows[i - 1]['time'])) / 1000.0 if dt > 0: - dv = (float(rows[i]['speed']) - float(rows[i-1]['speed'])) / 3.6 + dv = (float(rows[i]['speed']) - float(rows[i - 1]['speed'])) / 3.6 true_accel_g = (dv / dt) / 9.80665 else: true_accel_g = 0