Subtle filtering while keeping fast gyro response

[kristof]

Asymmetric Filtering

The key insight is that gyro and accelerometer serve different purposes in a balance robot:

Sensor	Purpose	Latency Requirement
Gyro	Balance control (fast reactions)	⚡ Ultra-low latency
Accel	Drift correction (slow, steady)	Can tolerate some smoothing

So the system uses asymmetric filtering — aggressive spike removal but minimal smoothing on gyro, while applying more smoothing to accelerometer.

---

Filter 1: Hampel Filter (Spike Removal)

class HampelFilter1D:
    """
    Streaming Hampel filter for spike removal.
    Keeps a rolling window; if the incoming value deviates from the median by
    > n_sigmas * 1.4826 * MAD, it is replaced with the median (or clamped).
    """
    def __init__(self, window_size=11, n_sigmas=3.5, clamp=False):
        # ...
    def filter(self, x):
        # ...
        if abs(x - med) > threshold:
            return med  # Replace spike with median
        return x  # Pass through normal values unchanged

How it minimizes latency:

• Small window size (5 samples) — only looks at recent history
• Pass-through for normal values — if the value isn't a spike, it returns immediately unchanged
• No phase delay — unlike low-pass filters, it doesn't smooth the signal, just removes outliers

The math: A value is a "spike" if it deviates from the median by more than 3.5 × 1.4826 × MAD (Median Absolute Deviation). This catches sensor glitches without affecting legitimate fast movements.

---

Filter 2: EMA (Exponential Moving Average)

class EMA1D:
    """ Simple exponential moving average for gentle smoothing. """
    def __init__(self, alpha=0.12):
        self.alpha = alpha
        self.y = None

    def filter(self, x):
        self.y = self.alpha * x + (1.0 - self.alpha) * self.y
        return self.y

Formula: output = α × new_value + (1-α) × previous_output

How it minimizes latency:

• High alpha values (0.3-0.4) = faster response, less smoothing
• Only applied where needed — disabled for gyro by default!

---

The Asymmetric Strategy

        hampel_window_size=5,      # small window for minimal latency
        hampel_sigmas=3.5,
        smooth_gyro=False,         # NO smoothing on gyro by default (fast response for balance)
        smooth_accel=True,         # light smoothing on accel is okay (used for drift correction)

Signal	Hampel (spike removal)	EMA (smoothing)	Result
Gyro	✅ Yes (window=5)	❌ No	Ultra-fast, spike-free
Accel	✅ Yes (window=5)	✅ Yes (α=0.3)	Smooth, stable

---

Architecture: Single-Slot Queue

        # Single-slot queue: always keep only the newest sample
        self.imu_queue = Queue(maxsize=1)

The background thread reads the IMU at the sampling frequency and always overwrites old data:

            # Non-blocking queue: keep only the newest
            try:
                if self.imu_queue.full():
                    _ = self.imu_queue.get_nowait()  # Discard old
                self.imu_queue.put_nowait(data)      # Put new

Why this minimizes latency:

• No queue buildup — you always get the freshest sample
• Non-blocking reads — get_data() never waits
• If no new data, returns the last known value (never blocks the control loop)

---

Visual Summary

Raw IMU Data (noisy, with occasional spikes)
         │
         ▼
   ┌─────────────┐
   │   Hampel    │  ← Removes spikes, passes normal values through
   │  Filter (5) │     Latency: ~0 for normal values
   └─────────────┘
         │
    ┌────┴────┐
    │         │
   GYRO     ACCEL
    │         │
    │    ┌────▼────┐
    │    │   EMA   │  ← Light smoothing (α=0.3)
    │    │ Filter  │     Only for drift correction
    │    └────┬────┘
    │         │
    ▼         ▼
 ⚡ FAST    SMOOTH
  (balance)  (drift)

The result: gyro responds instantly to body tilts (critical for not falling over), while accelerometer is smoothed to provide stable drift correction without being affected by high-frequency vibrations.