From 2d4f3dea53ad5d0c1a4c0f1fd06f1f6400d1650f Mon Sep 17 00:00:00 2001 From: ruv Date: Thu, 28 May 2026 23:29:14 -0400 Subject: [PATCH] feat(signal): ADR-143 RF-SLAM reflector discovery + anchor learning (#847) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - ruvsense/rf_slam.rs (forward-looking, ships v1 fixed-map first): - RfSlam::fixed_map() — discovery disabled (v1); with_discovery() — v2 - ReflectorObservation (CIR-tap sighting), PersistentReflector (per-axis Welford position, migration_m_per_day, classify Wall/Furniture/Mobile) - observe(): nearest-reflector association within assoc_radius or seed new; coherence-gated; static_anchors() rejects Mobile → ADR-139 ObjectAnchor set - persistent_count() for topology-change detection - 6 tests (fixed-map no-op, persistence, low-coherence reject, cluster split, mobile excluded, static→Wall); workspace 0 errors Co-Authored-By: claude-flow --- .../wifi-densepose-signal/src/ruvsense/mod.rs | 4 + .../src/ruvsense/rf_slam.rs | 301 ++++++++++++++++++ 2 files changed, 305 insertions(+) create mode 100644 v2/crates/wifi-densepose-signal/src/ruvsense/rf_slam.rs diff --git a/v2/crates/wifi-densepose-signal/src/ruvsense/mod.rs b/v2/crates/wifi-densepose-signal/src/ruvsense/mod.rs index c6640e87..bd2d4678 100644 --- a/v2/crates/wifi-densepose-signal/src/ruvsense/mod.rs +++ b/v2/crates/wifi-densepose-signal/src/ruvsense/mod.rs @@ -67,6 +67,9 @@ pub mod array_coordinator; // ADR-142: Evolution tracker + temporal VoxelMap (Bayesian, privacy-gated) pub mod evolution; +// ADR-143: RF-SLAM persistent reflector discovery + static-anchor learning +pub mod rf_slam; + // ADR-135: Empty-room baseline calibration (Welford online, circular phase) pub mod calibration; @@ -79,6 +82,7 @@ pub use array_coordinator::{ pub use evolution::{ ChangePoint, EvolutionTracker, TemporalVoxel, TemporalVoxelMap, VoxelGate, VoxelPrivacy, }; +pub use rf_slam::{PersistentReflector, ReflectorClass, ReflectorObservation, RfSlam}; pub use fusion_quality::{ CalibrationId, ContradictionFlag, EvidenceRef, FamilyId, QualityScore, }; diff --git a/v2/crates/wifi-densepose-signal/src/ruvsense/rf_slam.rs b/v2/crates/wifi-densepose-signal/src/ruvsense/rf_slam.rs new file mode 100644 index 00000000..dc4458ef --- /dev/null +++ b/v2/crates/wifi-densepose-signal/src/ruvsense/rf_slam.rs @@ -0,0 +1,301 @@ +//! ADR-143 — RF-SLAM: persistent reflector discovery and static-anchor learning. +//! +//! Ships **v1 fixed-map first** (known sensor positions + a small set of static +//! reflectors, `discovery_enabled = false`). v2 discovery — inferring persistent +//! reflector positions from ADR-134 CIR tap separation + temporal coherence, +//! clustering them into furniture/wall anchors, and detecting topology changes — +//! is gated behind `discovery_enabled` until a multi-day validation dataset is +//! collected (ADR-143 §2.5). +//! +//! Reflector positions, once discovered, are intended to land as ADR-139 +//! `WorldNode::ObjectAnchor` nodes; this module owns the inference, the +//! WorldGraph owns the persistence. + +use crate::ruvsense::field_model::WelfordStats; + +/// Classification of a discovered persistent reflector (mirrors ADR-139 +/// `AnchorKind`; kept local to avoid a crate dependency on the WorldGraph). +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ReflectorClass { + /// A near-static reflector consistent with a wall (very low migration). + Wall, + /// A slowly-moving reflector consistent with furniture. + Furniture, + /// Moves too fast to be a static anchor (rejected from the anchor set). + Mobile, +} + +/// A single CIR-tap-derived reflector sighting at a point in time (ADR-134 CIR). +#[derive(Debug, Clone, Copy)] +pub struct ReflectorObservation { + /// Inferred reflector position (east, north, up) in metres. + pub position: [f64; 3], + /// CIR dominant-tap delay (ns) that produced this sighting. + pub delay_ns: f64, + /// Temporal coherence of the tap in [0, 1] (gate quality). + pub coherence: f32, + /// Capture-clock time (ns). + pub at_ns: u64, +} + +/// A reflector accumulated over many sightings (ADR-143 §2). +#[derive(Debug, Clone)] +pub struct PersistentReflector { + /// Per-axis position statistics (Welford). + pos: [WelfordStats; 3], + /// Number of sightings folded in. + pub sightings: u64, + /// First and last sighting times (ns). + pub first_ns: u64, + /// Last sighting time (ns). + pub last_ns: u64, + /// Total displacement of the running mean since the first sighting (m). + cumulative_drift_m: f64, + /// Last mean position, for incremental drift accumulation. + last_mean: [f64; 3], +} + +impl PersistentReflector { + fn from_first(obs: &ReflectorObservation) -> Self { + let mut pos = [WelfordStats::new(), WelfordStats::new(), WelfordStats::new()]; + for a in 0..3 { + pos[a].update(obs.position[a]); + } + Self { + pos, + sightings: 1, + first_ns: obs.at_ns, + last_ns: obs.at_ns, + cumulative_drift_m: 0.0, + last_mean: obs.position, + } + } + + fn fold(&mut self, obs: &ReflectorObservation) { + for a in 0..3 { + self.pos[a].update(obs.position[a]); + } + let new_mean = self.mean_position(); + let d: f64 = (0..3).map(|a| (new_mean[a] - self.last_mean[a]).powi(2)).sum::().sqrt(); + self.cumulative_drift_m += d; + self.last_mean = new_mean; + self.last_ns = obs.at_ns; + self.sightings += 1; + } + + /// Mean reflector position. + #[must_use] + pub fn mean_position(&self) -> [f64; 3] { + [self.pos[0].mean, self.pos[1].mean, self.pos[2].mean] + } + + /// Positional spread (max per-axis std, m) — low ⇒ a stable reflector. + #[must_use] + pub fn position_std(&self) -> f64 { + (0..3).map(|a| self.pos[a].std_dev()).fold(0.0, f64::max) + } + + /// Mean-position migration rate in metres/day over the observed span. + #[must_use] + pub fn migration_m_per_day(&self) -> f64 { + let span_ns = self.last_ns.saturating_sub(self.first_ns); + if span_ns == 0 { + return 0.0; + } + let span_days = span_ns as f64 / 86_400_000_000_000.0; // ns → days + if span_days < 1e-9 { + return 0.0; + } + self.cumulative_drift_m / span_days + } + + /// Classify by migration rate (ADR-143 §2): walls barely move, furniture + /// migrates slowly, anything faster than `mobile_floor` m/day is rejected. + #[must_use] + pub fn classify(&self, wall_ceiling: f64, mobile_floor: f64) -> ReflectorClass { + let m = self.migration_m_per_day(); + if m <= wall_ceiling { + ReflectorClass::Wall + } else if m < mobile_floor { + ReflectorClass::Furniture + } else { + ReflectorClass::Mobile + } + } +} + +/// RF-SLAM reflector discovery engine (ADR-143). +#[derive(Debug, Clone)] +pub struct RfSlam { + reflectors: Vec, + /// Association radius (m): a sighting within this of a reflector's mean is + /// folded in; otherwise it seeds a new reflector. + assoc_radius_m: f64, + /// Minimum sightings before a reflector counts as "persistent". + min_sightings: u64, + /// Minimum tap coherence for a sighting to be admitted. + min_coherence: f32, + /// v2 discovery gate — false ⇒ fixed-map v1 (no new reflectors learned). + discovery_enabled: bool, +} + +impl RfSlam { + /// v1 fixed-map mode: discovery disabled. + #[must_use] + pub fn fixed_map() -> Self { + Self { + reflectors: Vec::new(), + assoc_radius_m: 0.5, + min_sightings: 20, + min_coherence: 0.6, + discovery_enabled: false, + } + } + + /// v2 discovery mode: learn persistent reflectors from sightings. + #[must_use] + pub fn with_discovery(assoc_radius_m: f64, min_sightings: u64, min_coherence: f32) -> Self { + Self { + reflectors: Vec::new(), + assoc_radius_m, + min_sightings, + min_coherence, + discovery_enabled: true, + } + } + + /// Whether v2 discovery is active. + #[must_use] + pub fn discovery_enabled(&self) -> bool { + self.discovery_enabled + } + + /// Ingest one CIR-derived sighting. In fixed-map mode this is a no-op + /// (returns false). In discovery mode it associates to the nearest reflector + /// within `assoc_radius_m` or seeds a new one; returns true if accepted. + pub fn observe(&mut self, obs: &ReflectorObservation) -> bool { + if !self.discovery_enabled || obs.coherence < self.min_coherence { + return false; + } + // Nearest-reflector association. + let mut best: Option<(usize, f64)> = None; + for (i, r) in self.reflectors.iter().enumerate() { + let m = r.mean_position(); + let d: f64 = (0..3).map(|a| (m[a] - obs.position[a]).powi(2)).sum::().sqrt(); + if d <= self.assoc_radius_m && best.map_or(true, |(_, bd)| d < bd) { + best = Some((i, d)); + } + } + match best { + Some((i, _)) => self.reflectors[i].fold(obs), + None => self.reflectors.push(PersistentReflector::from_first(obs)), + } + true + } + + /// Indices/refs of reflectors that have crossed the persistence threshold. + #[must_use] + pub fn persistent(&self) -> Vec<&PersistentReflector> { + self.reflectors.iter().filter(|r| r.sightings >= self.min_sightings).collect() + } + + /// Static-anchor set: persistent reflectors classified Wall or Furniture + /// (mobile reflectors rejected) — the candidate ADR-139 `ObjectAnchor`s. + #[must_use] + pub fn static_anchors(&self, wall_ceiling: f64, mobile_floor: f64) -> Vec<([f64; 3], ReflectorClass)> { + self.persistent() + .into_iter() + .map(|r| (r.mean_position(), r.classify(wall_ceiling, mobile_floor))) + .filter(|(_, c)| *c != ReflectorClass::Mobile) + .collect() + } + + /// Topology-change signal: the count of persistent reflectors. A caller + /// compares this across time; an increase/decrease beyond a threshold marks + /// a furniture-moved / room-changed event (ADR-143 §2 topology detection). + #[must_use] + pub fn persistent_count(&self) -> usize { + self.reflectors.iter().filter(|r| r.sightings >= self.min_sightings).count() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn obs(pos: [f64; 3], at_ns: u64) -> ReflectorObservation { + ReflectorObservation { position: pos, delay_ns: 10.0, coherence: 0.9, at_ns } + } + + #[test] + fn fixed_map_does_not_discover() { + let mut slam = RfSlam::fixed_map(); + assert!(!slam.discovery_enabled()); + assert!(!slam.observe(&obs([1.0, 1.0, 0.0], 0))); + assert_eq!(slam.persistent_count(), 0); + } + + #[test] + fn discovery_learns_persistent_reflector() { + let mut slam = RfSlam::with_discovery(0.5, 20, 0.6); + // 25 sightings clustered tightly around (2,3,0). + for i in 0..25u64 { + let jitter = if i % 2 == 0 { 0.01 } else { -0.01 }; + assert!(slam.observe(&obs([2.0 + jitter, 3.0, 0.0], i * 1_000_000))); + } + assert_eq!(slam.persistent_count(), 1); + let r = slam.persistent()[0]; + assert!((r.mean_position()[0] - 2.0).abs() < 0.05); + assert!(r.position_std() < 0.1); + } + + #[test] + fn low_coherence_sightings_rejected() { + let mut slam = RfSlam::with_discovery(0.5, 5, 0.6); + let mut o = obs([1.0, 1.0, 0.0], 0); + o.coherence = 0.3; // below min + assert!(!slam.observe(&o)); + assert_eq!(slam.persistent_count(), 0); + } + + #[test] + fn separate_clusters_form_distinct_reflectors() { + let mut slam = RfSlam::with_discovery(0.5, 3, 0.6); + for i in 0..5u64 { + slam.observe(&obs([0.0, 0.0, 0.0], i)); + slam.observe(&obs([5.0, 5.0, 0.0], i)); // > assoc_radius apart + } + assert_eq!(slam.persistent_count(), 2); + } + + #[test] + fn mobile_reflector_excluded_from_anchors() { + // A reflector whose mean marches ~10 m/day is Mobile, not an anchor. + let mut slam = RfSlam::with_discovery(50.0, 5, 0.6); + let day_ns = 86_400_000_000_000u64; + for i in 0..10u64 { + // Position advances 1 m each tenth-of-a-day → ~10 m/day. + let t = i * (day_ns / 10); + slam.observe(&obs([i as f64, 0.0, 0.0], t)); + } + let anchors = slam.static_anchors(0.05, 1.0); + assert!(anchors.is_empty(), "fast-migrating reflector must not be an anchor"); + // But it is still a persistent reflector (tracked, just not anchored). + assert_eq!(slam.persistent_count(), 1); + assert_eq!(slam.persistent()[0].classify(0.05, 1.0), ReflectorClass::Mobile); + } + + #[test] + fn static_reflector_classified_wall() { + let mut slam = RfSlam::with_discovery(0.5, 5, 0.6); + let day_ns = 86_400_000_000_000u64; + for i in 0..10u64 { + // Tight cluster, spanning ~1 day → ~0 migration. + let jitter = if i % 2 == 0 { 0.005 } else { -0.005 }; + slam.observe(&obs([3.0 + jitter, 0.0, 0.0], i * (day_ns / 10))); + } + let anchors = slam.static_anchors(0.05, 1.0); + assert_eq!(anchors.len(), 1); + assert_eq!(anchors[0].1, ReflectorClass::Wall); + } +}