diff --git a/shared/src/orisys_app_shared/force/__init__.py b/shared/src/orisys_app_shared/force/__init__.py index 485afea..2d61e45 100644 --- a/shared/src/orisys_app_shared/force/__init__.py +++ b/shared/src/orisys_app_shared/force/__init__.py @@ -13,6 +13,11 @@ from orisys_app_shared.force.calibration_recorder import ( CalibrationPoint, CalibrationRecorder, ) +from orisys_app_shared.force.filters import ( + ForceFilterChain, + FilterSettings, + settings_from_mapping, +) from orisys_app_shared.force.history import MetricHistory, PlotState, lowpass_filter from orisys_app_shared.force.session import ( NA, @@ -33,6 +38,8 @@ __all__ = [ "CalibrationPoint", "CalibrationRecorder", "DisplayForces", + "FilterSettings", + "ForceFilterChain", "MetricHistory", "PlotState", "SelectionStats", @@ -44,5 +51,6 @@ __all__ = [ "lowpass_filter", "meta_from_sensor_json", "select_display_forces", + "settings_from_mapping", "summarise", ] diff --git a/shared/src/orisys_app_shared/force/filters.py b/shared/src/orisys_app_shared/force/filters.py new file mode 100644 index 0000000..364c9d2 --- /dev/null +++ b/shared/src/orisys_app_shared/force/filters.py @@ -0,0 +1,205 @@ +"""One causal filter chain for the force readings, and the values it replaced. + +The chain runs **once, in the acquisition loop**, so the numbers on the screen +and the numbers in ``forces.csv`` are the same filtered values — there is no +second, display-only smoothing that would make the chart disagree with the +record. The unfiltered readings are kept beside the filtered ones, so a filter +can be judged instead of trusted. + +Both stages are causal and per-sample, because acquisition cannot look ahead: + +* **median** — a window of ``median_window`` samples (odd, ≥3), applied per + channel; it removes single-sample spikes without smearing a step; +* **low-pass** — a Butterworth IIR with persistent state (``sosfilt`` with + ``zi``), the same shape as the SDK's own ``LowpassFilter``. + +Order is median then low-pass: the median protects the IIR from a spike that +would otherwise ring for several samples. +""" + +from __future__ import annotations + +import json +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Iterable, Optional + +import numpy as np + +#: Channels of the chain: three raw counts followed by the three newton values. +CHANNELS = 6 + + +def _odd(value: Any, *, minimum: int = 3, maximum: int = 31) -> int: + try: + number = int(value) + except (TypeError, ValueError): + return 0 + if number < minimum: + return 0 + if number % 2 == 0: + number += 1 + return min(number, maximum) + + +@dataclass(frozen=True) +class FilterSettings: + """What the operator chose. ``enabled`` flags decide what actually runs.""" + + median_enabled: bool = False + median_window: int = 5 + lowpass_enabled: bool = True + lowpass_hz: float = 10.0 + lowpass_order: int = 2 + sample_rate_hz: float = 30.0 + #: Show the unfiltered readings next to the filtered ones on the cards. + show_unfiltered: bool = False + + def normalised(self) -> "FilterSettings": + window = _odd(self.median_window) + return FilterSettings( + median_enabled=bool(self.median_enabled) and window >= 3, + median_window=window or 5, + lowpass_enabled=bool(self.lowpass_enabled) and float(self.lowpass_hz) > 0, + lowpass_hz=max(0.0, float(self.lowpass_hz)), + lowpass_order=max(1, min(6, int(self.lowpass_order))), + sample_rate_hz=max(1.0, float(self.sample_rate_hz)), + show_unfiltered=bool(self.show_unfiltered), + ) + + def active(self) -> bool: + """Whether any stage actually filters (a disabled chain is a pass-through).""" + settings = self.normalised() + return bool(settings.median_enabled or settings.lowpass_enabled) + + def as_dict(self) -> dict: + settings = self.normalised() + return { + "median_enabled": settings.median_enabled, + "median_window": settings.median_window, + "lowpass_enabled": settings.lowpass_enabled, + "lowpass_hz": settings.lowpass_hz, + "lowpass_order": settings.lowpass_order, + "sample_rate_hz": settings.sample_rate_hz, + "show_unfiltered": settings.show_unfiltered, + } + + def describe(self) -> str: + """A short human line for the recording metadata.""" + settings = self.normalised() + parts = [] + if settings.median_enabled: + parts.append(f"median({settings.median_window})") + if settings.lowpass_enabled: + parts.append( + f"lowpass({settings.lowpass_hz:g}Hz,order{settings.lowpass_order})" + ) + return " + ".join(parts) if parts else "none" + + +def settings_from_mapping(data: Optional[dict]) -> FilterSettings: + """Build settings from a preferences/metadata mapping, with defaults.""" + source = dict(data or {}) + defaults = FilterSettings() + return FilterSettings( + median_enabled=source.get("median_enabled", defaults.median_enabled), + median_window=source.get("median_window", defaults.median_window), + lowpass_enabled=source.get("lowpass_enabled", defaults.lowpass_enabled), + lowpass_hz=source.get("lowpass_hz", defaults.lowpass_hz), + lowpass_order=source.get("lowpass_order", defaults.lowpass_order), + sample_rate_hz=source.get("sample_rate_hz", defaults.sample_rate_hz), + show_unfiltered=source.get("show_unfiltered", defaults.show_unfiltered), + ).normalised() + + +class ForceFilterChain: + """The median + low-pass chain, one instance per acquisition session.""" + + def __init__(self, settings: Optional[FilterSettings] = None) -> None: + self._settings = (settings or FilterSettings()).normalised() + self._median: list[list[float]] = [[] for _ in range(CHANNELS)] + self._sos = None + #: One IIR state per channel: the six readings are independent signals. + self._zi: Optional[list[np.ndarray]] = None + self._build() + + # ------------------------------------------------------------------ setup + def _build(self) -> None: + self._sos = None + self._zi = None + settings = self._settings + if not settings.lowpass_enabled: + return + from scipy.signal import butter + + nyquist = 0.5 * settings.sample_rate_hz + cutoff = min(float(settings.lowpass_hz), nyquist * 0.95) + if cutoff <= 0: + return + self._sos = butter( + settings.lowpass_order, + cutoff / nyquist, + btype="low", + output="sos", + ) + self._zi = None + + def configure(self, settings: FilterSettings) -> None: + """Re-tune the chain; the caller resets the stream when it does.""" + self._settings = settings.normalised() + self._median = [[] for _ in range(CHANNELS)] + self._build() + + def settings(self) -> FilterSettings: + return self._settings + + @property + def active(self) -> bool: + return self._settings.active() + + def reset(self, first: Optional[Iterable[float]] = None) -> None: + """Drop the filter memory (a new session must not inherit the old one).""" + self._median = [[] for _ in range(CHANNELS)] + self._zi = None + if first is not None and self._sos is not None: + # Start each IIR at its first sample's steady state instead of at + # zero: a step from 0 to a real reading otherwise rings for a few + # samples, which would show up as a transient at session start. + from scipy.signal import sosfilt_zi + + values = np.asarray(list(first), dtype=np.float64).reshape(-1) + if values.size == CHANNELS: + steady = sosfilt_zi(self._sos) + self._zi = [steady * float(value) for value in values] + + # ----------------------------------------------------------------- filter + def process(self, values) -> np.ndarray: + """Filter one 6-channel sample: three raw counts then three newtons.""" + sample = np.asarray(list(values), dtype=np.float64).reshape(-1) + if sample.size != CHANNELS: + raise ValueError(f"expected {CHANNELS} channels, got {sample.size}") + if not self.active: + return sample.copy() + settings = self._settings + out = sample.copy() + if settings.median_enabled: + window = int(settings.median_window) + for index in range(CHANNELS): + series = self._median[index] + series.append(float(sample[index])) + if len(series) > window: + del series[0] + out[index] = float(np.median(series)) + if self._sos is not None: + from scipy.signal import sosfilt + + if self._zi is None: + self.reset(out) + states = self._zi if self._zi is not None else [None] * CHANNELS + for index in range(CHANNELS): + filtered, states[index] = sosfilt( + self._sos, np.array([out[index]]), zi=states[index] + ) + out[index] = float(filtered[0]) + self._zi = states + return out diff --git a/shared/tests/test_force_filters.py b/shared/tests/test_force_filters.py new file mode 100644 index 0000000..2538225 --- /dev/null +++ b/shared/tests/test_force_filters.py @@ -0,0 +1,113 @@ +"""The force filter chain: what it removes, and what it must not do. + +The chain runs once, in the acquisition loop, so the numbers on the screen and +the numbers in ``forces.csv`` are the same filtered values; the unfiltered +readings are kept beside them. These tests pin the parts that are easy to get +wrong: odd windows, a disabled chain being a pass-through, per-channel state, +and a step not ringing at session start. +""" + +from __future__ import annotations + +import numpy as np + +from orisys_app_shared.force.filters import ( + CHANNELS, + FilterSettings, + ForceFilterChain, + settings_from_mapping, +) + + +def _zeros(value: float = 0.0) -> list[float]: + return [value] * CHANNELS + + +def test_a_disabled_chain_is_a_pass_through(): + chain = ForceFilterChain( + FilterSettings(median_enabled=False, lowpass_enabled=False) + ) + assert not chain.active + sample = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] + assert chain.process(sample).tolist() == sample + + +def test_the_median_removes_a_single_sample_spike(): + """One bad readout must not reach the chart, the store or the CSV.""" + chain = ForceFilterChain( + FilterSettings(median_enabled=True, median_window=3, lowpass_enabled=False) + ) + outputs = [ + float(chain.process(_zeros(9.0 if index == 2 else 0.0))[0]) + for index in range(5) + ] + assert outputs[2] == 0.0, "the spike survived the median" + # ...and a sustained change is *not* removed: this is not a clamp. + for _ in range(4): + sustained = float(chain.process(_zeros(9.0))[0]) + assert sustained == 9.0, sustained + + +def test_the_median_window_is_forced_odd_and_bounded(): + assert FilterSettings(median_window=4).normalised().median_window == 5 + assert FilterSettings(median_window=1).normalised().median_window == 5 + # A window below three samples cannot remove anything: the stage turns off. + assert FilterSettings(median_enabled=True, median_window=2).normalised().median_enabled is False + + +def test_the_lowpass_settles_on_a_step_without_ringing_at_the_start(): + """A session must not open with a transient invented by the filter.""" + chain = ForceFilterChain( + FilterSettings(lowpass_enabled=True, lowpass_hz=5.0, sample_rate_hz=30.0) + ) + first = float(chain.process(_zeros(3.0))[0]) + assert abs(first - 3.0) < 1e-9, f"the filter invented a transient: {first}" + # A step is damped and then reached. A second-order digital Butterworth + # overshoots a step by a few percent — that is its response, not a defect — + # so the bound is "a few percent", and the settled value is the step. + values = [ + float(chain.process(_zeros(0.0 if index < 5 else 3.0))[0]) + for index in range(80) + ] + assert max(values) <= 3.0 * 1.1, f"the low-pass overshot by more than 10%: {max(values)}" + assert values[-1] > 2.9, values[-1] + + +def test_each_channel_keeps_its_own_filter_state(): + chain = ForceFilterChain( + FilterSettings(median_enabled=True, median_window=3, lowpass_enabled=False) + ) + chain.process([1.0, 100.0, 2.0, 200.0, 3.0, 300.0]) + chain.process([1.0, 100.0, 2.0, 200.0, 3.0, 300.0]) + out = chain.process([1.0, 100.0, 2.0, 200.0, 3.0, 300.0]) + assert out.tolist() == [1.0, 100.0, 2.0, 200.0, 3.0, 300.0] + + +def test_reset_clears_the_memory_between_sessions(): + chain = ForceFilterChain( + FilterSettings(lowpass_enabled=True, lowpass_hz=5.0, sample_rate_hz=30.0) + ) + for _ in range(30): + chain.process(_zeros(5.0)) + chain.reset() + assert abs(float(chain.process(_zeros(0.0))[0])) < 1e-9, ( + "the new session inherited the previous filter state" + ) + + +def test_settings_round_trip_through_a_preferences_mapping(): + settings = settings_from_mapping( + { + "median_enabled": True, + "median_window": 7, + "lowpass_enabled": True, + "lowpass_hz": 12.5, + "show_unfiltered": True, + } + ) + assert settings.median_enabled and settings.median_window == 7 + assert settings.lowpass_hz == 12.5 and settings.show_unfiltered + assert settings.describe() == "median(7) + lowpass(12.5Hz,order2)" + # An empty mapping is the documented default, not a disabled chain. + default = settings_from_mapping(None) + assert default.as_dict()["lowpass_hz"] == FilterSettings().lowpass_hz