included low pass filter in package and add to plot_data example

src/orisys/signal_processing.py

@@ -0,0 +1,139 @@
+"""Signal helpers for force filtering and normal-force calibration.
+
+Batch ``lowpass_filter`` is the zero-phase Butterworth used by example charts.
+``LowpassFilter`` is a causal, stateful filter for per-frame ``Sensor`` outputs.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+from scipy.signal import butter, filtfilt, sosfilt, sosfilt_zi
+
+# Linear calibration from raw normal-force units to Newtons (ORY-BOX / ORY-MINI).
+NORMAL_FORCE_CAL_SLOPE = 3.92685554668795e-05
+NORMAL_FORCE_CAL_OFFSET = 0.0217596967278535
+
+DEFAULT_SAMPLE_RATE_HZ = 30.0
+DEFAULT_LOWPASS_ORDER = 2
+
+
+def calibrate_normal_force(raw_value: float) -> float:
+    """Convert a raw normal-force reading to Newtons using y = mx + c."""
+    return NORMAL_FORCE_CAL_SLOPE * float(raw_value) + NORMAL_FORCE_CAL_OFFSET
+
+
+def _nyquist_hz(sample_rate_hz: float) -> float:
+    return 0.5 * float(sample_rate_hz)
+
+
+def _validate_cutoff(cutoff_hz: float, sample_rate_hz: float) -> None:
+    nyquist = _nyquist_hz(sample_rate_hz)
+    if cutoff_hz < 0 or cutoff_hz >= nyquist:
+        raise ValueError(
+            f"cutoff_hz must be in (0, {nyquist}) for sample_rate_hz={sample_rate_hz}"
+        )
+
+
+def lowpass_filter(
+    data,
+    cutoff_hz: float = 5.0,
+    sample_rate_hz: float = DEFAULT_SAMPLE_RATE_HZ,
+    order: int = DEFAULT_LOWPASS_ORDER,
+) -> np.ndarray:
+    """Zero-phase Butterworth low-pass for uniformly sampled 1-D data.
+
+    Matches the example/viewer chart filter: ``cutoff_hz == 0`` is a no-op.
+    """
+    y = np.asarray(data, dtype=np.float64)
+    if cutoff_hz == 0:
+        return y.copy()
+    if y.size == 0:
+        return y
+
+    _validate_cutoff(cutoff_hz, sample_rate_hz)
+    b, a = butter(int(order), cutoff_hz / _nyquist_hz(sample_rate_hz), btype="low")
+    padlen = 3 * (max(len(a), len(b)) - 1)
+    if y.size <= padlen:
+        return y.copy()
+    try:
+        return filtfilt(b, a, y)
+    except ValueError:
+        return y.copy()
+
+
+class LowpassFilter:
+    """Causal Butterworth low-pass with persistent SOS state (one sample at a time)."""
+
+    def __init__(
+        self,
+        cutoff_hz: float = 0.0,
+        sample_rate_hz: float = DEFAULT_SAMPLE_RATE_HZ,
+        order: int = DEFAULT_LOWPASS_ORDER,
+        n_channels: int = 3,
+    ):
+        self.n_channels = int(n_channels)
+        self._sos = None
+        self._zi = None
+        self.configure(
+            cutoff_hz=cutoff_hz,
+            sample_rate_hz=sample_rate_hz,
+            order=order,
+        )
+
+    @property
+    def enabled(self) -> bool:
+        return self._sos is not None
+
+    def configure(
+        self,
+        *,
+        cutoff_hz: float | None = None,
+        sample_rate_hz: float | None = None,
+        order: int | None = None,
+    ) -> None:
+        if cutoff_hz is not None:
+            self.cutoff_hz = float(cutoff_hz)
+        if sample_rate_hz is not None:
+            self.sample_rate_hz = float(sample_rate_hz)
+        if order is not None:
+            self.order = int(order)
+
+        if self.cutoff_hz <= 0:
+            self._sos = None
+            self._zi = None
+            return
+
+        _validate_cutoff(self.cutoff_hz, self.sample_rate_hz)
+        self._sos = butter(
+            self.order,
+            self.cutoff_hz / _nyquist_hz(self.sample_rate_hz),
+            btype="low",
+            output="sos",
+        )
+        self._zi = None
+
+    def reset(self, x0=None) -> None:
+        """Clear filter memory. Optional ``x0`` initializes to a steady-state DC."""
+        if self._sos is None:
+            self._zi = None
+            return
+        if x0 is None:
+            self._zi = None
+            return
+        x = np.asarray(x0, dtype=np.float64).reshape(-1)
+        if x.size != self.n_channels:
+            raise ValueError(f"x0 must have {self.n_channels} channels, got {x.size}")
+        zi0 = sosfilt_zi(self._sos)
+        self._zi = zi0[:, :, None] * x[None, 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 self._sos is None:
+            return x.copy()
+        if self._zi is None:
+            self.reset(x)
+        y, self._zi = sosfilt(self._sos, x[None, :], zi=self._zi, axis=0)
+        return np.asarray(y[0], dtype=np.float64)