From 8fb6ef6547225296df66f417e9f57ab766f89006 Mon Sep 17 00:00:00 2001 From: ruv Date: Thu, 11 Jun 2026 21:00:19 -0400 Subject: [PATCH] =?UTF-8?q?fix(vitals):=20renormalize=20partial-weight=20f?= =?UTF-8?q?usion=20+=20clamp=20IIR=20resonator=20(ADR-157=20=C2=A7A2/?= =?UTF-8?q?=C2=A7A3)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit §A2 (correctness): BreathingExtractor weighted fusion was an un-normalized sum. When `weights` was supplied shorter than n, supplied entries were used raw while the missing tail defaulted to uniform 1/n -- two scales summed with no renormalization, silently mis-scaling the breathing signal by a factor of weights.len(). Extract to fuse_weighted_residuals() and normalize by Sigma(effective weights), mirroring heartrate::compute_phase_coherence_signal. Tests: partial_weights_are_renormalized_not_scale_mixed, partial_weights_fusion_is_weighted_average (both fail on old code). §A3 (stability): the IIR resonator pole radius r = 1 - bw/2 diverges when the pole MAGNITUDE |r| >= 1 (i.e. bw >= 4: a very low fs relative to band width) -- NOT merely when r is negative, as the research report stated (a negative r with |r| < 1 is still stable; the comments/tests are corrected accordingly). On divergence the filter overflows to +/-inf within ~600 frames, NaN-poisons acf0, and the extractor stalls permanently. Clamp r to [0, 0.9999] AND finite-guard the filter output before the history push (defense-in-depth, mirrors ADR-154 §3). Applied to both heartrate.rs and breathing.rs. Tests: {heartrate,breathing}::low_sample_rate_filter_stays_finite (fs=0.5, 0.1-0.9 Hz band, 600-frame unit step -> all-finite; both panic on old code). These files also carry the §A1 VecDeque window conversion (bit-identical). Co-Authored-By: claude-flow --- .../wifi-densepose-vitals/src/breathing.rs | 152 +++++++++++++++--- .../wifi-densepose-vitals/src/heartrate.rs | 77 +++++++-- 2 files changed, 193 insertions(+), 36 deletions(-) diff --git a/v2/crates/wifi-densepose-vitals/src/breathing.rs b/v2/crates/wifi-densepose-vitals/src/breathing.rs index 5010849e..5f375419 100644 --- a/v2/crates/wifi-densepose-vitals/src/breathing.rs +++ b/v2/crates/wifi-densepose-vitals/src/breathing.rs @@ -9,6 +9,7 @@ //! with weighted subcarrier fusion. use crate::types::{VitalEstimate, VitalStatus}; +use std::collections::VecDeque; /// IIR bandpass filter state (2nd-order resonator). #[derive(Clone, Debug)] @@ -32,8 +33,8 @@ impl Default for IirState { /// Respiratory rate extractor using bandpass filtering and zero-crossing analysis. pub struct BreathingExtractor { - /// Per-sample filtered signal history. - filtered_history: Vec, + /// Per-sample filtered signal history (sliding window; O(1) push/pop). + filtered_history: VecDeque, /// Sample rate in Hz. sample_rate: f64, /// Analysis window in seconds. @@ -59,7 +60,7 @@ impl BreathingExtractor { pub fn new(n_subcarriers: usize, sample_rate: f64, window_secs: f64) -> Self { let capacity = (sample_rate * window_secs) as usize; Self { - filtered_history: Vec::with_capacity(capacity), + filtered_history: VecDeque::with_capacity(capacity), sample_rate, window_secs, n_subcarriers, @@ -90,26 +91,27 @@ impl BreathingExtractor { return None; } - // Weighted fusion of subcarrier residuals - let uniform_w = 1.0 / n as f64; - let weighted_signal: f64 = residuals - .iter() - .enumerate() - .take(n) - .map(|(i, &r)| { - let w = weights.get(i).copied().unwrap_or(uniform_w); - r * w - }) - .sum(); + // Weighted fusion of subcarrier residuals (normalized — see + // `fuse_weighted_residuals`). + let weighted_signal = fuse_weighted_residuals(residuals, weights, n); // Apply IIR bandpass filter let filtered = self.bandpass_filter(weighted_signal); - // Append to history, enforce window limit - self.filtered_history.push(filtered); + // Defense-in-depth: never let a non-finite filter output (e.g. a + // diverged resonator pole at a pathological sample rate) enter the + // history buffer. Mirrors ADR-154 §3 / ADR-157 §A3. + if !filtered.is_finite() { + return None; + } + + // Append to history, enforce window limit. `VecDeque` gives O(1) + // push_back + pop_front for the sliding window (was a `Vec` with an + // O(n) `remove(0)` per sample — ADR-157 §A1). + self.filtered_history.push_back(filtered); let max_len = (self.sample_rate * self.window_secs) as usize; if self.filtered_history.len() > max_len { - self.filtered_history.remove(0); + self.filtered_history.pop_front(); } // Need at least 10 seconds of data @@ -118,9 +120,11 @@ impl BreathingExtractor { return None; } - // Zero-crossing rate -> frequency - let crossings = count_zero_crossings(&self.filtered_history); - let duration_s = self.filtered_history.len() as f64 / self.sample_rate; + // Zero-crossing rate -> frequency. `make_contiguous` rotates the ring + // buffer in place once so the slice helpers below can borrow it. + let history = self.filtered_history.make_contiguous(); + let crossings = count_zero_crossings(history); + let duration_s = history.len() as f64 / self.sample_rate; let frequency_hz = crossings as f64 / (2.0 * duration_s); // Validate frequency is within the breathing band @@ -129,7 +133,7 @@ impl BreathingExtractor { } let bpm = frequency_hz * 60.0; - let confidence = compute_confidence(&self.filtered_history); + let confidence = compute_confidence(history); let status = if confidence >= 0.7 { VitalStatus::Valid @@ -157,7 +161,14 @@ impl BreathingExtractor { let bw = omega_high - omega_low; let center = f64::midpoint(omega_low, omega_high); - let r = 1.0 - bw / 2.0; + // Clamp the resonator pole radius into a stable range. The pole + // magnitude is `|r|`; stability needs `|r| < 1`. When `bw` exceeds 4 + // (a very low `fs` relative to the band width) `1 - bw/2` drops below + // -1, pushing the pole outside the unit circle and diverging the filter + // exponentially to ±inf. (A merely-negative `r` with `|r| < 1` is still + // stable.) The clamp keeps the pole inside the unit circle for any + // sample-rate / band-edge configuration (ADR-157 §A3). + let r = (1.0 - bw / 2.0).clamp(0.0, 0.9999); let cos_w0 = center.cos(); let output = @@ -190,6 +201,32 @@ impl BreathingExtractor { } } +/// Fuse the first `n` per-subcarrier residuals into a single scalar using +/// the supplied attention `weights`, normalized by the sum of the +/// **effective** weights actually used. +/// +/// Missing weights (when `weights.len() < n`) default to the uniform weight +/// `1/n`. Normalizing by `Σ(effective weights)` is what makes a partial +/// `weights` slice safe: without it, supplied entries (used raw) and the +/// uniform tail are summed at two different scales, silently mis-scaling the +/// breathing signal. Mirrors `heartrate::compute_phase_coherence_signal` +/// (`weighted_sum / weight_total`). (ADR-157 §A2) +fn fuse_weighted_residuals(residuals: &[f64], weights: &[f64], n: usize) -> f64 { + let uniform_w = 1.0 / n as f64; + let mut weighted_sum = 0.0; + let mut weight_total = 0.0; + for (i, &r) in residuals.iter().enumerate().take(n) { + let w = weights.get(i).copied().unwrap_or(uniform_w); + weighted_sum += r * w; + weight_total += w; + } + if weight_total.abs() > 1e-15 { + weighted_sum / weight_total + } else { + 0.0 + } +} + /// Count zero crossings in a signal. fn count_zero_crossings(signal: &[f64]) -> usize { signal.windows(2).filter(|w| w[0] * w[1] < 0.0).count() @@ -310,4 +347,75 @@ mod tests { let ext = BreathingExtractor::esp32_default(); assert_eq!(ext.n_subcarriers, 56); } + + /// ADR-157 §A2 bug-catching test. + /// + /// With `residuals = [1.0; 8]` and `weights = [10.0, 10.0]` (len 2 < n=8), + /// the supplied weights (10.0) and the uniform-fallback tail (1/8) are at + /// two different scales. The correct, normalized fusion divides by the sum + /// of the *effective* weights, so the fused value must equal the + /// renormalized weighted mean of the residuals = 1.0 (all residuals equal + /// 1.0). The OLD code returned the un-normalized sum + /// (`2*10 + 6*0.125 = 20.75`), so this asserts the fix. + #[test] + fn partial_weights_are_renormalized_not_scale_mixed() { + let residuals = [1.0_f64; 8]; + let weights = [10.0_f64, 10.0]; + let fused = fuse_weighted_residuals(&residuals, &weights, 8); + + // Renormalized weighted mean of equal residuals is exactly the residual + // value, regardless of the weight scale. + assert!( + (fused - 1.0).abs() < 1e-12, + "partial weights must renormalize to the weighted mean (1.0), got {fused}" + ); + + // Explicitly pin that we are NOT returning the old scale-mixed sum. + let old_scale_mixed_sum: f64 = 2.0 * 10.0 + 6.0 * (1.0 / 8.0); + assert!( + (fused - old_scale_mixed_sum).abs() > 1.0, + "fused value must not equal the old un-normalized sum {old_scale_mixed_sum}" + ); + } + + /// ADR-157 §A2: with differing residual values, the normalized fusion is a + /// proper weighted average dominated by the high-weight entries. + #[test] + fn partial_weights_fusion_is_weighted_average() { + // Two heavily-weighted residuals of 2.0, the rest (uniform) of 0.0. + let residuals = [2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + let weights = [10.0_f64, 10.0]; + let fused = fuse_weighted_residuals(&residuals, &weights, 8); + // weighted_sum = 2*10*2 ... = 40; weight_total = 20 + 6*0.125 = 20.75 + let expected = (2.0 * 10.0 + 2.0 * 10.0) / (20.0 + 6.0 * 0.125); + assert!( + (fused - expected).abs() < 1e-12, + "expected weighted average {expected}, got {fused}" + ); + // Must lie within the residual range [0, 2] — a scale-mixed sum would not. + assert!((0.0..=2.0).contains(&fused), "weighted average must be in-range: {fused}"); + } + + /// ADR-157 §A3 bug-catching test. Divergence needs the pole magnitude + /// `|r| >= 1`, i.e. `bw >= 4`. At `fs = 0.5` Hz with the band widened to + /// 0.1-0.9 Hz, `bw = 2*pi*(0.9-0.1)/0.5 = 10.05`, so the OLD pole radius + /// `r = 1 - bw/2 = -4.03` has `|r| = 4.03 > 1` and the filter blows up + /// exponentially, overflowing to ±inf within ~600 unit-step frames. The + /// clamp + finite-guard keep every accumulated sample finite. This FAILS on + /// the old code (verified by reverting). + #[test] + fn low_sample_rate_filter_stays_finite() { + let mut ext = BreathingExtractor::new(4, 0.5, 3600.0); + ext.freq_low = 0.1; + ext.freq_high = 0.9; + // Feed a unit step for 600 frames — enough for the un-clamped resonator + // to overflow to inf. + for _ in 0..600 { + ext.extract(&[1.0, 1.0, 1.0, 1.0], &[0.25, 0.25, 0.25, 0.25]); + } + assert!(ext.history_len() > 0, "history should accumulate"); + for (i, &v) in ext.filtered_history.iter().enumerate() { + assert!(v.is_finite(), "filtered_history[{i}] must be finite, got {v}"); + } + } } diff --git a/v2/crates/wifi-densepose-vitals/src/heartrate.rs b/v2/crates/wifi-densepose-vitals/src/heartrate.rs index e3416ffa..72af98c8 100644 --- a/v2/crates/wifi-densepose-vitals/src/heartrate.rs +++ b/v2/crates/wifi-densepose-vitals/src/heartrate.rs @@ -10,6 +10,7 @@ //! than single-channel amplitude analysis. use crate::types::{VitalEstimate, VitalStatus}; +use std::collections::VecDeque; /// IIR bandpass filter state (2nd-order resonator). #[derive(Clone, Debug)] @@ -34,8 +35,8 @@ impl Default for IirState { /// Heart rate extractor using bandpass filtering and autocorrelation /// peak detection. pub struct HeartRateExtractor { - /// Per-sample filtered signal history. - filtered_history: Vec, + /// Per-sample filtered signal history (sliding window; O(1) push/pop). + filtered_history: VecDeque, /// Sample rate in Hz. sample_rate: f64, /// Analysis window in seconds. @@ -63,7 +64,7 @@ impl HeartRateExtractor { pub fn new(n_subcarriers: usize, sample_rate: f64, window_secs: f64) -> Self { let capacity = (sample_rate * window_secs) as usize; Self { - filtered_history: Vec::with_capacity(capacity), + filtered_history: VecDeque::with_capacity(capacity), sample_rate, window_secs, n_subcarriers, @@ -101,11 +102,21 @@ impl HeartRateExtractor { // Apply cardiac-band IIR bandpass filter let filtered = self.bandpass_filter(phase_signal); - // Append to history, enforce window limit - self.filtered_history.push(filtered); + // Defense-in-depth: a non-finite filter output (e.g. a diverged + // resonator pole at a pathological sample rate) must never enter the + // history buffer, or `acf0` would become NaN and the extractor would + // stall permanently. Mirrors the NaN-bypass guard in ADR-154 §3. + if !filtered.is_finite() { + return None; + } + + // Append to history, enforce window limit. `VecDeque` gives O(1) + // push_back + pop_front for the sliding window (was a `Vec` with an + // O(n) `remove(0)` per sample — ADR-157 §A1). + self.filtered_history.push_back(filtered); let max_len = (self.sample_rate * self.window_secs) as usize; if self.filtered_history.len() > max_len { - self.filtered_history.remove(0); + self.filtered_history.pop_front(); } // Need at least 5 seconds of data for cardiac detection @@ -114,13 +125,13 @@ impl HeartRateExtractor { return None; } - // Use autocorrelation to find the dominant periodicity - let (period_samples, acf_peak) = autocorrelation_peak( - &self.filtered_history, - self.sample_rate, - self.freq_low, - self.freq_high, - ); + // Use autocorrelation to find the dominant periodicity. The + // autocorrelation/peak loop needs a contiguous slice; `make_contiguous` + // rotates the ring buffer in place once per `extract()` so the slice is + // free for the rest of this call. + let history = self.filtered_history.make_contiguous(); + let (period_samples, acf_peak) = + autocorrelation_peak(history, self.sample_rate, self.freq_low, self.freq_high); if period_samples == 0 { return None; @@ -166,7 +177,15 @@ impl HeartRateExtractor { let bw = omega_high - omega_low; let center = f64::midpoint(omega_low, omega_high); - let r = 1.0 - bw / 2.0; + // Resonator pole radius. The pole magnitude is `|r|`; stability needs + // `|r| < 1`. When the normalized bandwidth `bw = 2*pi*(f_high-f_low)/fs` + // exceeds 4 (i.e. a very low `fs` relative to the band width), + // `1 - bw/2` falls below -1, pushing the pole *outside* the unit circle + // and diverging the filter exponentially to ±inf. A merely-negative `r` + // (|r| < 1) is still stable, so the clamp's job is the `|r| >= 1` case. + // Clamp to a stable range so the pole stays inside the unit circle for + // any `sample_rate` / band-edge configuration (ADR-157 §A3). + let r = (1.0 - bw / 2.0).clamp(0.0, 0.9999); let cos_w0 = center.cos(); let output = @@ -400,4 +419,34 @@ mod tests { let ext = HeartRateExtractor::esp32_default(); assert_eq!(ext.n_subcarriers, 56); } + + /// ADR-157 §A3 bug-catching test. + /// + /// Divergence needs the pole *magnitude* `|r| >= 1`, i.e. `bw >= 4`. With + /// the cardiac band widened to 0.1-0.9 Hz at `fs = 0.5` Hz, + /// `bw = 2*pi*(0.9-0.1)/0.5 = 10.05`, so the OLD pole radius + /// `r = 1 - bw/2 = -4.03` has `|r| = 4.03 > 1` — the filter diverges + /// exponentially. After ~600 unit-step frames the OLD output overflows f64 + /// to ±inf/NaN; once that lands in `filtered_history`, `acf0` becomes NaN + /// and the extractor stalls permanently. The clamp (`r.clamp(0.0, 0.9999)`) + /// plus the finite-guard before the push keep every accumulated sample + /// finite. This test FAILS on the old code (verified by reverting). + #[test] + fn low_sample_rate_filter_stays_finite() { + let mut ext = HeartRateExtractor::new(4, 0.5, 3600.0); + ext.freq_low = 0.1; + ext.freq_high = 0.9; + // Feed a unit step across 4 coherent subcarriers for 600 frames — enough + // for the un-clamped resonator to overflow to inf. + for _ in 0..600 { + ext.extract(&[1.0, 1.0, 1.0, 1.0], &[0.0, 0.01, 0.02, 0.03]); + } + assert!( + ext.history_len() > 0, + "history should have accumulated samples" + ); + for (i, &v) in ext.filtered_history.iter().enumerate() { + assert!(v.is_finite(), "filtered_history[{i}] must be finite, got {v}"); + } + } }