update filter, flow ema 0.5 + med 3

src/orisys/signal_processing.py

@@ -15,6 +15,8 @@ Prefer ``Sensor.calibration`` / ``read_info(FNORMAL, …)`` in new code.
 
 from __future__ import annotations
 
+import collections
+import time
 from dataclasses import dataclass
 
 import numpy as np
@@ -312,3 +314,115 @@ class LowpassFilter:
             self.reset(x)
         y, self._zi = sosfilt(self._sos, x[None, :], zi=self._zi, axis=0)
         return np.asarray(y[0], dtype=np.float64)
+
+
+class CausalMedianFilter:
+    """Causal sliding-median over the last ``window`` samples, one sample at a time.
+
+    The optical-flow force chain produces heavy-tailed jitter (rare multi-sigma
+    spikes). A linear low-pass spreads such a spike over several frames instead
+    of deleting it, whereas a median removes it outright - measured on idle
+    data, a 5-sample median cut the jitter 4x better than a moving average of
+    the same width.
+
+    ``window <= 1`` disables the filter (pass-through). Until ``window`` samples
+    have arrived the median of the available samples is returned, so the output
+    is well defined from the first frame.
+    """
+
+    def __init__(self, window: int = 1, n_channels: int = 3):
+        self.n_channels = int(n_channels)
+        self.window = 1
+        self._buf = None
+        self.configure(window)
+
+    @property
+    def enabled(self) -> bool:
+        return self.window > 1
+
+    def configure(self, window: int | None = None) -> None:
+        if window is not None:
+            try:
+                self.window = max(1, int(window))
+            except (TypeError, ValueError):
+                self.window = 1
+        self._buf = None
+
+    def reset(self, x0=None) -> None:
+        """Drop the window history (e.g. after a tracker reset)."""
+        self._buf = None
+
+    def process(self, values) -> np.ndarray:
+        """Filter one multi-channel sample. Returns float64 array of shape (n_channels,)."""
+        x = np.asarray(values, dtype=np.float64).reshape(-1)
+        if x.size != self.n_channels:
+            raise ValueError(f"expected {self.n_channels} channels, got {x.size}")
+        if not self.enabled:
+            return x.copy()
+        if self._buf is None or self._buf.shape[1] != x.size:
+            self._buf = x[None, :].copy()
+            return x.copy()
+        self._buf = np.vstack((self._buf, x[None, :]))[-self.window:]
+        return np.median(self._buf, axis=0)
+
+
+class SampleRateEstimator:
+    """Robust online estimate of the rate at which :meth:`tick` is called.
+
+    Uses the *median* of the most recent inter-call intervals, so one stall or
+    scheduling hiccup does not poison the estimate. :attr:`rate_hz` returns the
+    seed value until ``min_samples`` intervals have been observed.
+
+    This exists because a fixed floor filter designed for 30 Hz behaves
+    differently at 60 Hz: the cutoff stays put in Hz but the per-frame
+    smoothing halves. Measuring the achieved rate keeps the filter identical in
+    the time domain whatever the deployment runs at.
+    """
+
+    def __init__(
+        self,
+        initial_hz: float = 30.0,
+        window: int = 45,
+        min_samples: int = 8,
+        max_interval_s: float = 1.0,
+    ):
+        self.initial_hz = float(initial_hz) if initial_hz else 30.0
+        self.window = max(3, int(window))
+        self.min_samples = max(2, int(min_samples))
+        self.max_interval_s = float(max_interval_s)
+        self.reset()
+
+    def reset(self, initial_hz: float | None = None) -> None:
+        if initial_hz is not None:
+            self.initial_hz = float(initial_hz) if initial_hz else 30.0
+        self._dts: collections.deque = collections.deque(maxlen=self.window)
+        self._last = None
+        self._rate = self.initial_hz
+        self._measured = False
+
+    @property
+    def rate_hz(self) -> float:
+        return self._rate
+
+    @property
+    def measured(self) -> bool:
+        """True once the estimate comes from real intervals."""
+        return self._measured
+
+    def tick(self, now: float | None = None) -> float:
+        """Register one sample and return the current rate estimate."""
+        now = time.perf_counter() if now is None else float(now)
+        last, self._last = self._last, now
+        if last is None:
+            return self._rate
+        dt = now - last
+        if not (1e-5 < dt <= self.max_interval_s):
+            # Stalls, pauses and the first frame carry no rate information.
+            return self._rate
+        self._dts.append(dt)
+        if len(self._dts) >= self.min_samples:
+            dt_med = float(np.median(self._dts))
+            if dt_med > 0.0:
+                self._rate = 1.0 / dt_med
+                self._measured = True
+        return self._rate