ee830d8505
把同帧测量、空载归零、模型推断与共用后处理收进一个 SDK 会话,Studio 显示与记录使用同一样本;旧 Sensor 读数接口保持兼容。以 force session 与受影响 SDK 套件、lint 与文档门禁验证,硬件与真实载荷精度未验证。 Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01CpCfJF6SvonMKmPZhzKZHb
704 lines
28 KiB
Python
704 lines
28 KiB
Python
"""Force readout for FN/FX/FY: EMA, median, low-pass, and optional convert.
|
|
|
|
Any sensor that produces a raw (fn, fx, fy) triple can use this. Calibration
|
|
coefficients stay outside: pass ``convert`` into :meth:`ForceReadout.apply`.
|
|
|
|
Two things live here, deliberately apart:
|
|
|
|
* :class:`ForceReadout` — the historical measurement-chain readout. It filters,
|
|
then converts (median → low-pass → convert), which is not interchangeable with
|
|
a nonlinear conversion; existing consumers keep it unchanged.
|
|
* :class:`ForceOutputFilter` — the shared *output* post-processing for the new
|
|
session path: causal median → low-pass → optional per-axis threshold, applied
|
|
to the raw features and to the unfiltered newtons as two independent states.
|
|
Conversion happens upstream of it, so it can run on both domains.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from collections.abc import Callable
|
|
from dataclasses import dataclass, replace
|
|
|
|
import numpy as np
|
|
|
|
from .signal_processing import (
|
|
DEFAULT_SAMPLE_RATE_HZ,
|
|
CausalMedianFilter,
|
|
LowpassFilter,
|
|
MultiAxisCausalFilter,
|
|
SampleRateEstimator,
|
|
)
|
|
|
|
# Default signal-conditioning settings (config: top-level "filters" block).
|
|
#
|
|
# A key a config does not state is **off**, not "whatever we thought was
|
|
# sensible": the file has to name what runs, or a device can be filtering a
|
|
# measurement nobody asked it to filter and no reader of the file could tell.
|
|
# The shipped bundles state all three values explicitly, so they are unaffected
|
|
# by these defaults; a config with no "filters" block at all filters nothing.
|
|
#
|
|
# Which settings are worth asking for is measured, not guessed. On dd02-0703 at
|
|
# 280x300 (see the noise analysis in the perf notes) flow EMA 0.5 + median 3
|
|
# costs ~2 frames at 30 fps and cuts steady-press force jitter by roughly half
|
|
# while also stabilising vnormal/contact (which a force-level filter cannot do);
|
|
# that combination is the "balanced" preset. The force low-pass stays off there
|
|
# too: on top of the EMA it added no measurable noise benefit at the same
|
|
# latency, so it is an opt-in stage for sites with different noise
|
|
# characteristics (the "low_noise" preset).
|
|
DEFAULT_FORCE_LOWPASS_HZ = 0.0
|
|
DEFAULT_FORCE_MEDIAN_WINDOW = 1
|
|
DEFAULT_FLOW_EMA_ALPHA = 1.0
|
|
|
|
#: Named filter presets, so a recommendation is *executable* instead of a
|
|
#: comment in a JSON file (``json.load`` cannot carry comments). A config may
|
|
#: write ``"filters": {"preset": "balanced"}`` and override any single key on
|
|
#: top of it; the resolved values are what ``Sensor.filters_state()`` reports.
|
|
#: The latency/noise trade-offs behind these numbers are measured in
|
|
#: ``docs/performance/perf-measurements/`` (moved out of the once-gitignored
|
|
#: ``orisys-sdk/_perf/`` on 2026-09-27) and tabulated in ``docs/filters.md``.
|
|
FILTER_PRESETS = {
|
|
# Everything off: the file says what runs, and the raw reading is the record.
|
|
"off": {
|
|
"flow_ema_alpha": 1.0,
|
|
"force_median_window": 1,
|
|
"force_lowpass_cutoff_hz": 0.0,
|
|
},
|
|
# One frame of latency @30 fps: median only. The EMA changes the
|
|
# measurement, so it stays off when latency matters more than jitter.
|
|
"low_latency": {
|
|
"flow_ema_alpha": 1.0,
|
|
"force_median_window": 3,
|
|
"force_lowpass_cutoff_hz": 0.0,
|
|
},
|
|
# Two frames @30 fps: the EMA removes the spatially-coherent flow jitter
|
|
# that no force-level filter can reach, the median removes its spikes.
|
|
"balanced": {
|
|
"flow_ema_alpha": 0.5,
|
|
"force_median_window": 3,
|
|
"force_lowpass_cutoff_hz": 0.0,
|
|
},
|
|
# Three frames @30 fps and the quietest readout: as ``balanced`` plus the
|
|
# force low-pass on top of the median.
|
|
"low_noise": {
|
|
"flow_ema_alpha": 0.3,
|
|
"force_median_window": 3,
|
|
"force_lowpass_cutoff_hz": 10.0,
|
|
},
|
|
}
|
|
|
|
|
|
def available_filter_presets() -> dict:
|
|
"""The presets, for UIs that want to offer them (the studio's gear dialog)."""
|
|
return {name: dict(values) for name, values in FILTER_PRESETS.items()}
|
|
|
|
|
|
ConvertFn = Callable[[float, float, float], tuple[float, float, float]]
|
|
|
|
|
|
class ForceReadout:
|
|
"""Median → low-pass → convert for a three-channel force triple."""
|
|
|
|
def __init__(
|
|
self,
|
|
filters_cfg: dict | None = None,
|
|
*,
|
|
config_fps: float = DEFAULT_SAMPLE_RATE_HZ,
|
|
verbose: bool = False,
|
|
) -> None:
|
|
# These settings change the reported force values, so they are part of
|
|
# the measurement-pipeline identity and are recorded in the calibration
|
|
# metadata / mismatch check alongside dis_mode and hhd_downsample.
|
|
#
|
|
# Every stage is switched by its **own value**, so the config states what
|
|
# runs: ``flow_ema_alpha >= 1``, ``force_median_window <= 1`` and
|
|
# ``force_lowpass_cutoff_hz <= 0`` each mean "stage off". Values are never
|
|
# overridden: there is no master switch that could make the file say 0.5
|
|
# while the device runs 1.0. An invalid value falls back to **off**, never
|
|
# to a default that filters. ``enabled`` is kept only as a deprecated
|
|
# alias for "turn every stage off" and is honoured when it is explicitly
|
|
# false. ``state()`` reports what actually took effect.
|
|
if not isinstance(filters_cfg, dict):
|
|
filters_cfg = {}
|
|
legacy_off = filters_cfg.get("enabled", None) is False
|
|
preset_name = filters_cfg.get("preset", None)
|
|
preset_values = FILTER_PRESETS.get(str(preset_name), {}) if preset_name else {}
|
|
self.preset = str(preset_name) if preset_name in FILTER_PRESETS else None
|
|
|
|
def _cfg(key, default):
|
|
if legacy_off:
|
|
return default
|
|
value = filters_cfg.get(key, None)
|
|
if value is None:
|
|
value = preset_values.get(key, None)
|
|
return default if value is None else value
|
|
|
|
try:
|
|
self.config_fps = float(config_fps or 0.0)
|
|
except (TypeError, ValueError):
|
|
self.config_fps = 0.0
|
|
if self.config_fps <= 0:
|
|
self.config_fps = float(DEFAULT_SAMPLE_RATE_HZ)
|
|
|
|
self.lowpass_order = 2
|
|
self.lowpass_sample_rate_hz = self.config_fps
|
|
self.lowpass_cutoff_hz = 0.0
|
|
self.lowpass_rate_auto = True
|
|
try:
|
|
self.lowpass_order = max(1, int(_cfg("force_lowpass_order", 2)))
|
|
except (TypeError, ValueError):
|
|
self.lowpass_order = 2
|
|
rate_cfg = _cfg("force_lowpass_sample_rate_hz", "auto")
|
|
if isinstance(rate_cfg, str):
|
|
self.lowpass_rate_auto = rate_cfg.strip().lower() in ("auto", "", "measured")
|
|
else:
|
|
try:
|
|
rate = float(rate_cfg)
|
|
except (TypeError, ValueError):
|
|
rate = 0.0
|
|
self.lowpass_rate_auto = rate <= 0.0
|
|
if not self.lowpass_rate_auto:
|
|
self.lowpass_sample_rate_hz = rate
|
|
try:
|
|
self.lowpass_cutoff_hz = float(
|
|
_cfg("force_lowpass_cutoff_hz", DEFAULT_FORCE_LOWPASS_HZ)
|
|
)
|
|
except (TypeError, ValueError):
|
|
self.lowpass_cutoff_hz = 0.0
|
|
if self.lowpass_sample_rate_hz <= 0:
|
|
self.lowpass_sample_rate_hz = self.config_fps
|
|
# An unusable cutoff is "off", never a silent jump to the default.
|
|
if not (0.0 <= self.lowpass_cutoff_hz < 0.5 * self.lowpass_sample_rate_hz):
|
|
self.lowpass_cutoff_hz = 0.0
|
|
self.lowpass_enabled = self.lowpass_cutoff_hz > 0
|
|
self._force_filter = LowpassFilter(
|
|
cutoff_hz=self.lowpass_cutoff_hz,
|
|
sample_rate_hz=self.lowpass_sample_rate_hz,
|
|
order=self.lowpass_order,
|
|
n_channels=3,
|
|
)
|
|
self._force_rate = SampleRateEstimator(initial_hz=self.config_fps)
|
|
self.sample_rate_source = "config"
|
|
|
|
try:
|
|
self.force_median_window = max(
|
|
1, int(_cfg("force_median_window", DEFAULT_FORCE_MEDIAN_WINDOW))
|
|
)
|
|
except (TypeError, ValueError):
|
|
self.force_median_window = 1
|
|
self._force_median = CausalMedianFilter(
|
|
window=self.force_median_window, n_channels=3
|
|
)
|
|
|
|
self.flow_ema_alpha = 1.0
|
|
try:
|
|
alpha = float(_cfg("flow_ema_alpha", DEFAULT_FLOW_EMA_ALPHA))
|
|
except (TypeError, ValueError):
|
|
alpha = 1.0
|
|
if 0.0 < alpha < 1.0:
|
|
self.flow_ema_alpha = alpha
|
|
self._ema_flow = None
|
|
if legacy_off and verbose:
|
|
print(
|
|
"Note: filters.enabled is deprecated and means 'all stages off'; "
|
|
"remove it and switch each stage by its own value "
|
|
"(flow_ema_alpha=1, force_median_window=1, force_lowpass_cutoff_hz=0)."
|
|
)
|
|
self.any_stage_applied = bool(
|
|
self.flow_ema_alpha < 1.0
|
|
or self.force_median_window > 1
|
|
or self.lowpass_enabled
|
|
)
|
|
self.filtered_raw: tuple[float, float, float] | None = None
|
|
self.raw_unfiltered: tuple[float, float, float] | None = None
|
|
self.unfiltered_newton: tuple[float, float, float] | None = None
|
|
|
|
@property
|
|
def sample_rate_hz(self) -> float:
|
|
return float(self.lowpass_sample_rate_hz)
|
|
|
|
def pin_file_rate(self) -> None:
|
|
"""Wall-clock cadence is not signal time (video file / offline replay)."""
|
|
self.lowpass_rate_auto = False
|
|
self.sample_rate_source = "config (file)"
|
|
self.lowpass_sample_rate_hz = self.config_fps
|
|
self._force_rate.reset(self.config_fps)
|
|
|
|
def apply_flow_ema(self, flow):
|
|
"""Temporally smooth the optical-flow field before HHD.
|
|
|
|
The frame-to-frame jitter of the estimated flow is spatially coherent
|
|
(a near-global shift: it survives ~40x less averaging than per-pixel
|
|
noise would, see the noise analysis), so it cannot be removed by
|
|
spatial smoothing, a coarser HHD grid or a different DIS setting - only
|
|
by averaging along time. Applying it here also stabilises
|
|
``vnormal``/contact, which a force-level filter cannot reach.
|
|
|
|
``flow_ema_alpha >= 1`` disables the stage.
|
|
"""
|
|
alpha = self.flow_ema_alpha
|
|
if flow is None or alpha >= 1.0:
|
|
return flow
|
|
prev = self._ema_flow
|
|
if prev is None or prev.shape != flow.shape or prev.dtype != flow.dtype:
|
|
self._ema_flow = np.array(flow, copy=True)
|
|
return flow
|
|
prev += alpha * (flow - prev)
|
|
return prev
|
|
|
|
def update_sample_rate(self) -> None:
|
|
"""Track the achieved force update rate for the low-pass design."""
|
|
if not self.lowpass_rate_auto:
|
|
return
|
|
hz = self._force_rate.tick()
|
|
if not (1.0 <= hz <= 240.0):
|
|
return
|
|
self.sample_rate_source = "measured"
|
|
used = self.lowpass_sample_rate_hz
|
|
if used > 0 and abs(hz - used) <= 0.1 * used:
|
|
return
|
|
self.lowpass_sample_rate_hz = hz
|
|
if self.lowpass_enabled:
|
|
self._force_filter.configure(
|
|
cutoff_hz=self.lowpass_cutoff_hz,
|
|
sample_rate_hz=hz,
|
|
order=self.lowpass_order,
|
|
)
|
|
|
|
def apply(
|
|
self,
|
|
raw,
|
|
convert: ConvertFn | None = None,
|
|
) -> tuple[float, float, float]:
|
|
"""Median → low-pass → optional convert. Returns filtered newton triple."""
|
|
pre_filter = np.asarray(raw, dtype=np.float64)
|
|
values = pre_filter
|
|
# Median first: it deletes the heavy-tailed spikes that the flow chain
|
|
# produces, so the low-pass does not have to spread them over time.
|
|
if self._force_median.enabled:
|
|
values = self._force_median.process(values)
|
|
if self.lowpass_enabled and self.lowpass_cutoff_hz > 0:
|
|
values = self._force_filter.process(values)
|
|
self.filtered_raw = (float(values[0]), float(values[1]), float(values[2]))
|
|
fn, fx, fy = self.filtered_raw
|
|
if convert is not None:
|
|
fn, fx, fy = convert(fn, fx, fy)
|
|
fn, fx, fy = float(fn), float(fx), float(fy)
|
|
|
|
ufn, ufx, ufy = (
|
|
float(pre_filter[0]),
|
|
float(pre_filter[1]),
|
|
float(pre_filter[2]),
|
|
)
|
|
self.raw_unfiltered = (ufn, ufx, ufy)
|
|
if convert is not None:
|
|
ufn, ufx, ufy = convert(ufn, ufx, ufy)
|
|
ufn, ufx, ufy = float(ufn), float(ufx), float(ufy)
|
|
self.unfiltered_newton = (ufn, ufx, ufy)
|
|
return fn, fx, fy
|
|
|
|
def configure(
|
|
self,
|
|
*,
|
|
lowpass_cutoff_hz=None,
|
|
lowpass_sample_rate_hz=None,
|
|
lowpass_order=None,
|
|
lowpass_enabled=None,
|
|
flow_ema_alpha=None,
|
|
force_median_window=None,
|
|
) -> bool:
|
|
"""Update filter settings. Returns True when the force filter changed."""
|
|
filter_changed = False
|
|
if lowpass_cutoff_hz is not None:
|
|
self.lowpass_cutoff_hz = float(lowpass_cutoff_hz)
|
|
filter_changed = True
|
|
if lowpass_enabled is None:
|
|
self.lowpass_enabled = self.lowpass_cutoff_hz > 0
|
|
if lowpass_sample_rate_hz is not None:
|
|
# Accept a positive number to pin the rate, or "auto"/<=0 to track
|
|
# the measured output rate (see update_sample_rate).
|
|
auto = False
|
|
rate = 0.0
|
|
if isinstance(lowpass_sample_rate_hz, str):
|
|
auto = lowpass_sample_rate_hz.strip().lower() in ("auto", "", "measured")
|
|
else:
|
|
try:
|
|
rate = float(lowpass_sample_rate_hz)
|
|
except (TypeError, ValueError):
|
|
auto = True
|
|
else:
|
|
auto = rate <= 0.0
|
|
self.lowpass_rate_auto = auto
|
|
if not auto:
|
|
self.lowpass_sample_rate_hz = rate
|
|
elif self.lowpass_sample_rate_hz <= 0:
|
|
self.lowpass_sample_rate_hz = self.config_fps
|
|
self._force_rate.reset(self.config_fps)
|
|
filter_changed = True
|
|
if lowpass_order is not None:
|
|
order = int(lowpass_order)
|
|
if order < 1:
|
|
raise ValueError("lowpass_order must be >= 1")
|
|
self.lowpass_order = order
|
|
filter_changed = True
|
|
if lowpass_enabled is not None:
|
|
self.lowpass_enabled = bool(lowpass_enabled)
|
|
filter_changed = True
|
|
|
|
if flow_ema_alpha is not None:
|
|
alpha = float(flow_ema_alpha)
|
|
if not (0.0 < alpha <= 1.0):
|
|
raise ValueError("flow_ema_alpha must be in (0, 1]")
|
|
self.flow_ema_alpha = alpha
|
|
self._ema_flow = None
|
|
|
|
if force_median_window is not None:
|
|
try:
|
|
window = max(1, int(force_median_window))
|
|
except (TypeError, ValueError) as exc:
|
|
raise ValueError("force_median_window must be an integer >= 1") from exc
|
|
self.force_median_window = window
|
|
self._force_median.configure(window)
|
|
filter_changed = True
|
|
|
|
if filter_changed:
|
|
cutoff = self.lowpass_cutoff_hz if self.lowpass_enabled else 0.0
|
|
self._force_filter.configure(
|
|
cutoff_hz=cutoff,
|
|
sample_rate_hz=self.lowpass_sample_rate_hz,
|
|
order=self.lowpass_order,
|
|
)
|
|
self.any_stage_applied = bool(
|
|
self.flow_ema_alpha < 1.0
|
|
or self.force_median_window > 1
|
|
or self.lowpass_enabled
|
|
)
|
|
return filter_changed
|
|
|
|
def reset(self) -> None:
|
|
self._force_filter.reset()
|
|
self._force_median.reset()
|
|
self._ema_flow = None
|
|
self.filtered_raw = None
|
|
self.raw_unfiltered = None
|
|
self.unfiltered_newton = None
|
|
|
|
def state(self, *, dis_mode=None) -> dict:
|
|
"""Effective filter chain, for records and audits."""
|
|
return {
|
|
"preset": self.preset,
|
|
"measurement": {
|
|
"flow_ema_alpha": float(self.flow_ema_alpha),
|
|
"flow_ema_applied": bool(self.flow_ema_alpha < 1.0),
|
|
"dis_mode": dis_mode,
|
|
},
|
|
"readout": {
|
|
"force_median_window": int(self.force_median_window),
|
|
"force_median_applied": bool(self._force_median.enabled),
|
|
"force_lowpass_cutoff_hz": float(self.lowpass_cutoff_hz),
|
|
"force_lowpass_order": int(self.lowpass_order),
|
|
"force_lowpass_sample_rate_hz": float(self.lowpass_sample_rate_hz),
|
|
"force_lowpass_rate_auto": bool(self.lowpass_rate_auto),
|
|
"force_lowpass_applied": bool(self.lowpass_enabled),
|
|
"sample_rate_source": str(self.sample_rate_source),
|
|
},
|
|
"any_stage_applied": bool(self.any_stage_applied),
|
|
"unfiltered_available": True,
|
|
}
|
|
|
|
|
|
FORCE_AXES: tuple[str, str, str] = ("fn", "fx", "fy")
|
|
|
|
|
|
def _positive_float(value, default: float) -> float:
|
|
try:
|
|
number = float(value)
|
|
except (TypeError, ValueError):
|
|
return float(default)
|
|
if not np.isfinite(number) or number <= 0:
|
|
return float(default)
|
|
return number
|
|
|
|
|
|
def _non_negative_float(value, default: float = 0.0) -> float:
|
|
try:
|
|
number = float(value)
|
|
except (TypeError, ValueError):
|
|
return float(default)
|
|
if not np.isfinite(number) or number < 0:
|
|
return float(default)
|
|
return number
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class OutputSettings:
|
|
"""One resolved setting group for BOTH display and recording.
|
|
|
|
Every stage is switched by its own value: ``median_window=1`` and
|
|
``lowpass_hz=0`` each mean "off", and a threshold of 0 disables that unit's
|
|
threshold. All stages off is an exact pass-through (a NaN axis stays NaN).
|
|
This does not switch off the upstream flow EMA; that belongs to the
|
|
measurement context and is part of the calibration identity.
|
|
|
|
``sample_rate_hz`` describes the *signal* time of the input frames, declared
|
|
by the caller (camera config or recorded signal clock). The offline
|
|
processing speed never designs the low-pass. A cutoff at or above Nyquist is
|
|
unusable and resolves to "off" rather than to a silent re-tune.
|
|
"""
|
|
|
|
sample_rate_hz: float
|
|
median_window: int = 1
|
|
lowpass_hz: float = 0.0
|
|
lowpass_order: int = 2
|
|
raw_threshold: float = 0.0
|
|
newton_threshold: float = 0.0
|
|
|
|
def normalised(self) -> OutputSettings:
|
|
"""Clamp to values the chain can actually run; unusable values are "off"."""
|
|
rate = _positive_float(self.sample_rate_hz, DEFAULT_SAMPLE_RATE_HZ)
|
|
try:
|
|
window = max(1, int(self.median_window))
|
|
except (TypeError, ValueError):
|
|
window = 1
|
|
cutoff = _non_negative_float(self.lowpass_hz)
|
|
if cutoff >= 0.5 * rate:
|
|
cutoff = 0.0
|
|
try:
|
|
order = max(1, int(self.lowpass_order))
|
|
except (TypeError, ValueError):
|
|
order = 2
|
|
return replace(
|
|
self,
|
|
sample_rate_hz=rate,
|
|
median_window=window,
|
|
lowpass_hz=cutoff,
|
|
lowpass_order=order,
|
|
raw_threshold=_non_negative_float(self.raw_threshold),
|
|
newton_threshold=_non_negative_float(self.newton_threshold),
|
|
)
|
|
|
|
@property
|
|
def median_on(self) -> bool:
|
|
return int(self.median_window) > 1
|
|
|
|
@property
|
|
def lowpass_on(self) -> bool:
|
|
return float(self.lowpass_hz) > 0
|
|
|
|
def state(self) -> dict:
|
|
"""Effective parameters, for a record or an audit."""
|
|
return {
|
|
"sample_rate_hz": float(self.sample_rate_hz),
|
|
"median_window": int(self.median_window),
|
|
"median_applied": bool(self.median_on),
|
|
"lowpass_hz": float(self.lowpass_hz),
|
|
"lowpass_applied": bool(self.lowpass_on),
|
|
"lowpass_order": int(self.lowpass_order),
|
|
"raw_threshold": float(self.raw_threshold),
|
|
"newton_threshold": float(self.newton_threshold),
|
|
}
|
|
|
|
|
|
class ForceOutputFilter:
|
|
"""Median → low-pass → optional thresholds, once, for raw and N separately.
|
|
|
|
One state per domain: two :class:`MultiAxisCausalFilter` instances with the
|
|
same settings. The session hands each frame's features here exactly once, so
|
|
display and recording read the same processed sample; there is no second
|
|
filter state for a consumer to drift from.
|
|
|
|
Invalid axes (NaN or an undeclared valid flag) never enter the filter
|
|
history and stay NaN, which keeps "missing" distinct from a valid zero. With
|
|
every stage off the input tuples are returned unchanged, NaN included.
|
|
"""
|
|
|
|
def __init__(self, settings: OutputSettings):
|
|
self._settings = settings.normalised()
|
|
self._chain_settings = self._settings
|
|
self._raw_chain = self._build_chain()
|
|
self._newton_chain = self._build_chain()
|
|
self._rate = SampleRateEstimator(initial_hz=self._settings.sample_rate_hz)
|
|
|
|
# ------------------------------------------------------------- settings
|
|
def _build_chain(self) -> MultiAxisCausalFilter:
|
|
settings = self._settings
|
|
return MultiAxisCausalFilter(
|
|
median_window=settings.median_window,
|
|
lowpass_cutoff_hz=settings.lowpass_hz,
|
|
sample_rate_hz=settings.sample_rate_hz,
|
|
lowpass_order=settings.lowpass_order,
|
|
)
|
|
|
|
@property
|
|
def settings(self) -> OutputSettings:
|
|
"""The effective settings currently applied (rate included)."""
|
|
return self._settings
|
|
|
|
@property
|
|
def active(self) -> bool:
|
|
"""Any stage would change a valid reading."""
|
|
return bool(
|
|
self._raw_chain.enabled
|
|
or self._settings.raw_threshold > 0
|
|
or self._settings.newton_threshold > 0
|
|
)
|
|
|
|
def configure(self, settings: OutputSettings) -> bool:
|
|
"""Replace the setting group. State is dropped; the caller resets the stream."""
|
|
resolved = settings.normalised()
|
|
if resolved == self._settings:
|
|
return False
|
|
self._settings = resolved
|
|
self._raw_chain = self._build_chain()
|
|
self._newton_chain = self._build_chain()
|
|
self._rate.reset(resolved.sample_rate_hz)
|
|
return True
|
|
|
|
def reset(self) -> None:
|
|
"""Drop all filter memory (new zero, model change, discontinuity)."""
|
|
self._raw_chain.reset()
|
|
self._newton_chain.reset()
|
|
self._rate.reset(self._settings.sample_rate_hz)
|
|
|
|
# ---------------------------------------------------------- processing
|
|
def _track_signal_rate(self, signal_time_s: float | None) -> None:
|
|
"""Design the low-pass from signal time, never from processing speed."""
|
|
if signal_time_s is None:
|
|
return
|
|
if not np.isfinite(float(signal_time_s)):
|
|
return
|
|
rate = self._rate.tick(float(signal_time_s))
|
|
if not self._rate.measured:
|
|
return
|
|
current = self._settings.sample_rate_hz
|
|
if current > 0 and abs(rate - current) <= 0.1 * current:
|
|
return
|
|
self._settings = replace(self._settings, sample_rate_hz=rate)
|
|
self._raw_chain.configure(sample_rate_hz=rate)
|
|
self._newton_chain.configure(sample_rate_hz=rate)
|
|
|
|
def _apply_chain(
|
|
self,
|
|
chain: MultiAxisCausalFilter,
|
|
values,
|
|
valid,
|
|
*,
|
|
threshold: float,
|
|
) -> tuple[float, float, float]:
|
|
raw = tuple(float(value) for value in values)
|
|
if valid is None:
|
|
usable = np.isfinite(np.asarray(raw, dtype=np.float64))
|
|
else:
|
|
usable = np.asarray(valid, dtype=bool).reshape(-1)
|
|
if usable.size != 3:
|
|
raise ValueError(f"expected 3 validity flags, got {usable.size}")
|
|
processed = chain.process(raw, valid=usable)
|
|
out: list[float] = []
|
|
for index in range(3):
|
|
value = float(processed[index])
|
|
if not usable[index] or not np.isfinite(value):
|
|
out.append(float("nan"))
|
|
continue
|
|
if threshold > 0 and abs(value) <= threshold:
|
|
out.append(0.0)
|
|
else:
|
|
out.append(value)
|
|
return (out[0], out[1], out[2])
|
|
|
|
def process(
|
|
self,
|
|
raw,
|
|
newton,
|
|
*,
|
|
raw_valid=None,
|
|
newton_valid=None,
|
|
signal_time_s: float | None = None,
|
|
) -> tuple[tuple[float, float, float], tuple[float, float, float]]:
|
|
"""Process one frame's raw and newton triplets; returns both outputs."""
|
|
self._track_signal_rate(signal_time_s)
|
|
if not self.active:
|
|
# Exact pass-through, NaN and signed zero included.
|
|
return (
|
|
tuple(float(value) for value in raw),
|
|
tuple(float(value) for value in newton),
|
|
)
|
|
raw_out = self._apply_chain(
|
|
self._raw_chain,
|
|
raw,
|
|
raw_valid,
|
|
threshold=self._settings.raw_threshold,
|
|
)
|
|
newton_out = self._apply_chain(
|
|
self._newton_chain,
|
|
newton,
|
|
newton_valid,
|
|
threshold=self._settings.newton_threshold,
|
|
)
|
|
return raw_out, newton_out
|
|
|
|
def state(self) -> dict:
|
|
return {
|
|
"settings": self._settings.state(),
|
|
"raw_chain": self._raw_chain.state(),
|
|
"newton_chain": self._newton_chain.state(),
|
|
"sample_rate_source": (
|
|
"signal_time" if self._rate.measured else "declared_signal_rate"
|
|
),
|
|
}
|
|
|
|
|
|
def process_output_sequence(
|
|
settings: OutputSettings,
|
|
raw_samples,
|
|
newton_samples,
|
|
*,
|
|
raw_valid=None,
|
|
newton_valid=None,
|
|
signal_times_s=None,
|
|
) -> tuple[list[tuple[float, float, float]], list[tuple[float, float, float]], OutputSettings]:
|
|
"""Re-process stored unfiltered raw/N sequences through the SAME output chain.
|
|
|
|
This is the offline entry a replay/analysis pass uses to re-filter saved
|
|
unfiltered samples without a second median/low-pass implementation: it runs
|
|
the live chain in order from a clean state, including warm-up, so the result
|
|
equals what the session would have produced for that exact sample sequence.
|
|
|
|
``signal_times_s`` should carry the recorded signal times (the low-pass is
|
|
designed from signal time, never from how fast the re-analysis runs). When it
|
|
is omitted the declared ``settings.sample_rate_hz`` stays in effect. Returns
|
|
one output triplet per input sample, plus the effective settings.
|
|
"""
|
|
raw_rows = list(raw_samples)
|
|
newton_rows = list(newton_samples)
|
|
if len(raw_rows) != len(newton_rows):
|
|
raise ValueError(
|
|
"raw_samples and newton_samples must have the same length: "
|
|
f"{len(raw_rows)} != {len(newton_rows)}"
|
|
)
|
|
count = len(raw_rows)
|
|
raw_flags = None if raw_valid is None else list(raw_valid)
|
|
newton_flags = None if newton_valid is None else list(newton_valid)
|
|
times = None if signal_times_s is None else list(signal_times_s)
|
|
for name, rows in (
|
|
("raw_valid", raw_flags),
|
|
("newton_valid", newton_flags),
|
|
("signal_times_s", times),
|
|
):
|
|
if rows is not None and len(rows) != count:
|
|
raise ValueError(
|
|
f"{name} must have one entry per sample: {len(rows)} != {count}"
|
|
)
|
|
chain = ForceOutputFilter(settings)
|
|
raw_out: list[tuple[float, float, float]] = []
|
|
newton_out: list[tuple[float, float, float]] = []
|
|
for index in range(count):
|
|
processed_raw, processed_newton = chain.process(
|
|
raw_rows[index],
|
|
newton_rows[index],
|
|
raw_valid=None if raw_flags is None else raw_flags[index],
|
|
newton_valid=None if newton_flags is None else newton_flags[index],
|
|
signal_time_s=None if times is None else float(times[index]),
|
|
)
|
|
raw_out.append(processed_raw)
|
|
newton_out.append(processed_newton)
|
|
return raw_out, newton_out, chain.settings
|