diff --git a/v2/crates/wifi-densepose-calibration/src/lib.rs b/v2/crates/wifi-densepose-calibration/src/lib.rs index 4e8f21aa..18a841ac 100644 --- a/v2/crates/wifi-densepose-calibration/src/lib.rs +++ b/v2/crates/wifi-densepose-calibration/src/lib.rs @@ -25,11 +25,13 @@ pub mod extract; pub mod specialist; pub mod bank; pub mod runtime; +pub mod multistatic; pub use anchor::{Anchor, AnchorLabel, AnchorQuality, EnrollmentEvent, EnrollmentSession, Posture}; pub use bank::SpecialistBank; pub use enrollment::{AnchorQualityGate, AnchorRecorder}; pub use error::{CalibrationError, Result}; pub use extract::AnchorFeature; +pub use multistatic::MultiNodeMixture; pub use runtime::{MixtureOfSpecialists, RoomState}; pub use specialist::{Specialist, SpecialistKind, SpecialistReading}; diff --git a/v2/crates/wifi-densepose-calibration/src/multistatic.rs b/v2/crates/wifi-densepose-calibration/src/multistatic.rs new file mode 100644 index 00000000..4c52a55b --- /dev/null +++ b/v2/crates/wifi-densepose-calibration/src/multistatic.rs @@ -0,0 +1,265 @@ +//! Multistatic fusion (ADR-029 / ADR-151) — combine several *co-located* nodes +//! observing one room. +//! +//! More links = more geometric diversity, so a person hidden from one node's +//! line of sight is caught by another. Each node carries its own room-calibrated +//! [`SpecialistBank`] (its own baseline + anchors); this fuses their per-window +//! readings into a single [`RoomState`]: +//! +//! - **presence** — OR across nodes (any node seeing a person wins); +//! - **posture / breathing / heartbeat** — the highest-*confidence* node (best +//! viewpoint for that signal that window); +//! - **restlessness** — max (any node detecting movement); +//! - **anomaly / veto** — max / any (a single implausible node vetoes the room); +//! - **stale** — any node's bank stale flags the fused result. +//! +//! This is *same-room* multistatic. Nodes in *different* rooms are a federation +//! concern (ADR-105), not fusion — see ADR-151 §3.3. + +use std::collections::BTreeMap; + +use crate::bank::SpecialistBank; +use crate::extract::Features; +use crate::runtime::{MixtureOfSpecialists, RoomState}; +use crate::specialist::SpecialistReading; + +/// A bank plus the node's current baseline id (for per-node staleness). +struct NodeEntry { + mixture: MixtureOfSpecialists, + baseline_id: String, +} + +/// Fuses co-located nodes' specialist banks into one room state. +#[derive(Default)] +pub struct MultiNodeMixture { + nodes: BTreeMap, +} + +impl MultiNodeMixture { + /// Empty fusion set. + pub fn new() -> Self { + Self { + nodes: BTreeMap::new(), + } + } + + /// Register a node's bank. `current_baseline_id` is the baseline the node is + /// observing now (drift vs the bank's training baseline → STALE). + pub fn add_node(&mut self, node_id: u8, bank: SpecialistBank, current_baseline_id: impl Into) { + self.nodes.insert( + node_id, + NodeEntry { + mixture: MixtureOfSpecialists::new(bank), + baseline_id: current_baseline_id.into(), + }, + ); + } + + /// Number of registered nodes. + pub fn node_count(&self) -> usize { + self.nodes.len() + } + + /// Fuse per-node feature windows into one room state. Nodes without a feature + /// entry this window are skipped. + pub fn infer(&self, per_node: &BTreeMap) -> RoomState { + let states: Vec = per_node + .iter() + .filter_map(|(id, f)| { + self.nodes + .get(id) + .map(|e| e.mixture.infer(f, &e.baseline_id)) + }) + .collect(); + + if states.is_empty() { + return RoomState::default(); + } + + let presence = fuse_presence(&states); + let anomaly = max_value(states.iter().map(|s| &s.anomaly)); + // Conservative: a single node seeing a physically-implausible signal + // vetoes the room (anti-hallucination, same as the single-node runtime). + let vetoed = states.iter().any(|s| s.vetoed); + let present = presence.as_ref().map(|r| r.value > 0.5).unwrap_or(true); + + // Vitals/posture only when present and not vetoed. + let (posture, breathing, heartbeat) = if present && !vetoed { + ( + best_confidence(states.iter().map(|s| &s.posture)), + best_confidence(states.iter().map(|s| &s.breathing)), + best_confidence(states.iter().map(|s| &s.heartbeat)), + ) + } else { + (None, None, None) + }; + + RoomState { + presence, + posture, + breathing, + heartbeat, + restlessness: max_value(states.iter().map(|s| &s.restlessness)), + anomaly, + vetoed, + stale: states.iter().any(|s| s.stale), + } + } +} + +/// Presence: a person is present if ANY node sees one; confidence = max. +fn fuse_presence(states: &[RoomState]) -> Option { + let readings: Vec<&SpecialistReading> = states.iter().filter_map(|s| s.presence.as_ref()).collect(); + if readings.is_empty() { + return None; + } + let any_present = readings.iter().any(|r| r.value > 0.5); + let confidence = readings + .iter() + .map(|r| r.confidence) + .fold(0.0f32, f32::max); + Some(SpecialistReading { + kind: readings[0].kind, + value: if any_present { 1.0 } else { 0.0 }, + confidence, + label: Some(if any_present { "present" } else { "absent" }.into()), + }) +} + +/// Pick the highest-confidence reading across nodes. +fn best_confidence<'a>( + readings: impl Iterator>, +) -> Option { + readings + .flatten() + .fold(None::<&SpecialistReading>, |best, r| match best { + Some(b) if b.confidence >= r.confidence => Some(b), + _ => Some(r), + }) + .cloned() +} + +/// Pick the reading with the maximum value across nodes (movement / anomaly). +fn max_value<'a>( + readings: impl Iterator>, +) -> Option { + readings + .flatten() + .fold(None::<&SpecialistReading>, |best, r| match best { + Some(b) if b.value >= r.value => Some(b), + _ => Some(r), + }) + .cloned() +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::anchor::AnchorLabel; + use crate::extract::AnchorFeature; + + fn af(label: AnchorLabel, variance: f32, motion: f32) -> AnchorFeature { + AnchorFeature { + room_id: "r".into(), + label, + features: Features { + mean: 1.0, + variance, + motion, + breathing_score: 0.0, + breathing_hz: 0.0, + heart_score: 0.0, + heart_hz: 0.0, + }, + } + } + + fn bank(baseline: &str) -> SpecialistBank { + let anchors = vec![ + af(AnchorLabel::Empty, 1.0, 0.1), + af(AnchorLabel::StandStill, 10.0, 0.2), + af(AnchorLabel::Sit, 6.0, 0.2), + af(AnchorLabel::SmallMove, 4.0, 1.2), + af(AnchorLabel::SleepPosture, 3.0, 0.1), + ]; + SpecialistBank::train("r", baseline, &anchors, 1).unwrap() + } + + fn live(variance: f32, motion: f32, br_hz: f32, br_score: f32) -> Features { + Features { + mean: 1.0, + variance, + motion, + breathing_score: br_score, + breathing_hz: br_hz, + heart_score: 0.0, + heart_hz: 0.0, + } + } + + #[test] + fn two_nodes_register() { + let mut m = MultiNodeMixture::new(); + m.add_node(1, bank("b1"), "b1"); + m.add_node(2, bank("b2"), "b2"); + assert_eq!(m.node_count(), 2); + } + + #[test] + fn presence_or_across_nodes() { + let mut m = MultiNodeMixture::new(); + m.add_node(1, bank("b1"), "b1"); + m.add_node(2, bank("b1"), "b1"); + // Node 1 sees nobody (low variance), node 2 sees a person (high variance). + let mut per = BTreeMap::new(); + per.insert(1u8, live(1.0, 0.1, 0.0, 0.0)); + per.insert(2u8, live(12.0, 0.2, 0.3, 0.9)); + let s = m.infer(&per); + assert_eq!(s.presence.unwrap().value, 1.0, "any node present → present"); + assert!(s.breathing.is_some()); + } + + #[test] + fn breathing_picks_best_confidence_node() { + let mut m = MultiNodeMixture::new(); + m.add_node(1, bank("b1"), "b1"); + m.add_node(2, bank("b1"), "b1"); + let mut per = BTreeMap::new(); + // Both present; node 2 has the stronger breathing periodicity. + per.insert(1u8, live(12.0, 0.2, 0.2, 0.4)); + per.insert(2u8, live(12.0, 0.2, 0.3, 0.95)); + let s = m.infer(&per); + let br = s.breathing.unwrap(); + assert!((br.value - 18.0).abs() < 0.3, "picked 0.3 Hz node"); + assert!(br.confidence > 0.9); + } + + #[test] + fn anomaly_in_one_node_vetoes_room() { + let mut m = MultiNodeMixture::new(); + m.add_node(1, bank("b1"), "b1"); + m.add_node(2, bank("b1"), "b1"); + let mut per = BTreeMap::new(); + per.insert(1u8, live(12.0, 0.2, 0.3, 0.9)); + per.insert(2u8, live(9000.0, 500.0, 0.0, 0.0)); // wild outlier + let s = m.infer(&per); + assert!(s.vetoed); + assert!(s.breathing.is_none()); + } + + #[test] + fn stale_node_flags_room() { + let mut m = MultiNodeMixture::new(); + m.add_node(1, bank("b1"), "b2"); // trained on b1, now observing b2 → stale + let mut per = BTreeMap::new(); + per.insert(1u8, live(12.0, 0.2, 0.3, 0.9)); + assert!(m.infer(&per).stale); + } + + #[test] + fn empty_window_safe() { + let m = MultiNodeMixture::new(); + let s = m.infer(&BTreeMap::new()); + assert!(s.presence.is_none()); + } +}