diff --git a/v2/crates/wifi-densepose-mat/src/domain/disaster_event.rs b/v2/crates/wifi-densepose-mat/src/domain/disaster_event.rs index 52a4cd35..6cdefb2c 100644 --- a/v2/crates/wifi-densepose-mat/src/domain/disaster_event.rs +++ b/v2/crates/wifi-densepose-mat/src/domain/disaster_event.rs @@ -274,18 +274,40 @@ impl DisasterEvent { self.scan_zones.retain(|z| z.id() != zone_id); } - /// Record a new detection + /// Record a new detection. + /// + /// Deduplication is two-tiered so that the same trapped person re-detected + /// across successive scan cycles is updated in place rather than counted as a + /// new survivor (which would fabricate a mass-casualty event): + /// + /// 1. **Spatial** — if the detection has a real `location`, match an existing + /// survivor within `LOCATION_DEDUP_RADIUS_M`. + /// 2. **Zone + vitals-signature** — if there is NO usable location (no + /// multi-node geometry / RSSI available, which is the common edge case + /// for a single-node deployment), match an existing *active* survivor in + /// the SAME zone whose most recent vital-sign signature is compatible + /// (same breathing presence and rate band, same heartbeat presence, same + /// movement class). Without this, every scan cycle would push a brand new + /// survivor for the one person actually present. + /// + /// This is conservative on the safety side: two genuinely distinct survivors + /// in the same zone with materially different vitals (e.g. different + /// breathing-rate bands, or one with a pulse and one without) are kept + /// separate; only readings that are plausibly the same person collapse. pub fn record_detection( &mut self, zone_id: ScanZoneId, vitals: VitalSignsReading, location: Option, ) -> Result<&Survivor, MatError> { - // Check if this might be an existing survivor + // Tier 1: spatial dedup when a real location is available. let existing_id = if let Some(loc) = &location { - self.find_nearby_survivor(loc, 2.0).cloned() + self.find_nearby_survivor(loc, Self::LOCATION_DEDUP_RADIUS_M) + .cloned() } else { - None + // Tier 2: zone + vitals-signature dedup when location is unavailable. + self.find_matching_survivor_by_signature(&zone_id, &vitals) + .cloned() }; if let Some(existing) = existing_id { @@ -312,6 +334,10 @@ impl DisasterEvent { .expect("survivors is non-empty after push")) } + /// Radius (metres) within which a located detection is treated as the same + /// survivor for spatial deduplication. + const LOCATION_DEDUP_RADIUS_M: f64 = 2.0; + /// Find a survivor near a location fn find_nearby_survivor(&self, location: &Coordinates3D, radius: f64) -> Option<&SurvivorId> { for survivor in &self.survivors { @@ -324,6 +350,79 @@ impl DisasterEvent { None } + /// Find an existing *active*, *un-located* survivor in the same zone whose + /// most-recent vital signature is compatible with `vitals`. + /// + /// Only survivors without a fixed location participate: a survivor that has + /// a known position is handled by spatial dedup, and collapsing a located + /// survivor into an un-located reading would lose information. Returns the + /// first compatible match (there is normally at most one un-located survivor + /// per zone precisely because this dedup keeps it from multiplying). + fn find_matching_survivor_by_signature( + &self, + zone_id: &ScanZoneId, + vitals: &VitalSignsReading, + ) -> Option<&SurvivorId> { + for survivor in &self.survivors { + if survivor.zone_id() != zone_id { + continue; + } + if survivor.location().is_some() { + continue; + } + if !matches!( + survivor.status(), + super::survivor::SurvivorStatus::Active | super::survivor::SurvivorStatus::Lost + ) { + continue; + } + if let Some(latest) = survivor.vital_signs().latest() { + if Self::vitals_signature_matches(latest, vitals) { + return Some(survivor.id()); + } + } + } + None + } + + /// Decide whether two vital-sign readings are plausibly the same person. + /// + /// Matches on coarse, detection-stable features rather than exact values + /// (CSI-derived rates jitter cycle-to-cycle): breathing presence + rate band, + /// heartbeat presence, and movement class. Breathing rate is bucketed into + /// START-relevant bands (<10, 10–30, >30 bpm) with a small tolerance so a + /// breath rate hovering near a band edge does not split one person in two. + fn vitals_signature_matches(a: &VitalSignsReading, b: &VitalSignsReading) -> bool { + // Breathing presence must agree. + if a.breathing.is_some() != b.breathing.is_some() { + return false; + } + if let (Some(ba), Some(bb)) = (&a.breathing, &b.breathing) { + // Same START rate band, with a 1.5 bpm tolerance at band edges. + const EDGE_TOL: f32 = 1.5; + let band = |r: f32| -> i8 { + if r < 10.0 - EDGE_TOL { + 0 + } else if r > 30.0 + EDGE_TOL { + 2 + } else { + 1 + } + }; + if band(ba.rate_bpm) != band(bb.rate_bpm) { + return false; + } + } + + // Heartbeat presence must agree. + if a.heartbeat.is_some() != b.heartbeat.is_some() { + return false; + } + + // Movement class must agree. + a.movement.movement_type == b.movement.movement_type + } + /// Get survivor by ID pub fn get_survivor(&self, id: &SurvivorId) -> Option<&Survivor> { self.survivors.iter().find(|s| s.id() == id) @@ -486,4 +585,63 @@ mod tests { < DisasterType::Earthquake.expected_survival_hours() ); } + + /// Count-inflation regression (FAILS on the old code, which returned 3). + /// + /// Three detections of the SAME person (identical vitals, no usable location + /// because no multi-node geometry is available) must collapse to a single + /// survivor. Previously, `record_detection` only deduplicated when a location + /// was present, so an un-located trapped person re-detected every scan cycle + /// produced N survivors — a fabricated mass-casualty count. + #[test] + fn test_identical_vitals_no_location_dedup_to_one() { + let mut event = DisasterEvent::new(DisasterType::Earthquake, Point::new(0.0, 0.0), "Test"); + let zone = ScanZone::new("Zone A", ZoneBounds::rectangle(0.0, 0.0, 10.0, 10.0)); + let zone_id = zone.id().clone(); + event.add_zone(zone); + + for _ in 0..3 { + event + .record_detection(zone_id.clone(), create_test_vitals(), None) + .unwrap(); + } + + assert_eq!( + event.survivors().len(), + 1, + "same un-located person detected 3x must be ONE survivor, not three" + ); + } + + /// Counterpart: two genuinely DIFFERENT survivors in the same zone (different + /// breathing-rate bands) must remain separate — dedup must not under-count. + #[test] + fn test_distinct_vitals_no_location_stay_separate() { + let mut event = DisasterEvent::new(DisasterType::Earthquake, Point::new(0.0, 0.0), "Test"); + let zone = ScanZone::new("Zone A", ZoneBounds::rectangle(0.0, 0.0, 10.0, 10.0)); + let zone_id = zone.id().clone(); + event.add_zone(zone); + + // Person 1: normal breathing (16 bpm band 1). + event + .record_detection(zone_id.clone(), create_test_vitals(), None) + .unwrap(); + + // Person 2: tachypneic breathing (38 bpm band 2) — distinct survivor. + let fast = VitalSignsReading { + breathing: Some(BreathingPattern { + rate_bpm: 38.0, + amplitude: 0.8, + regularity: 0.5, + pattern_type: BreathingType::Labored, + }), + heartbeat: None, + movement: Default::default(), + timestamp: Utc::now(), + confidence: ConfidenceScore::new(0.8), + }; + event.record_detection(zone_id, fast, None).unwrap(); + + assert_eq!(event.survivors().len(), 2); + } } diff --git a/v2/crates/wifi-densepose-mat/src/domain/scan_zone.rs b/v2/crates/wifi-densepose-mat/src/domain/scan_zone.rs index 35f7bff6..980b22ed 100644 --- a/v2/crates/wifi-densepose-mat/src/domain/scan_zone.rs +++ b/v2/crates/wifi-densepose-mat/src/domain/scan_zone.rs @@ -265,6 +265,12 @@ pub struct SensorPosition { pub sensor_type: SensorType, /// Whether sensor is operational pub is_operational: bool, + /// Most recent measured RSSI (dBm) from this sensor toward the current + /// detection, when available from real hardware. `None` means no live + /// signal-strength reading is plumbed for this sensor (e.g. single-node + /// deployment or simulated zone) — localization will not fabricate one. + #[cfg_attr(feature = "serde", serde(default))] + pub last_rssi: Option, } /// Types of sensors @@ -482,6 +488,7 @@ mod tests { z: 1.5, sensor_type: SensorType::Transceiver, is_operational: true, + last_rssi: None, }); } diff --git a/v2/crates/wifi-densepose-mat/src/integration/hardware_adapter.rs b/v2/crates/wifi-densepose-mat/src/integration/hardware_adapter.rs index 00ee4d65..a8d75a95 100644 --- a/v2/crates/wifi-densepose-mat/src/integration/hardware_adapter.rs +++ b/v2/crates/wifi-densepose-mat/src/integration/hardware_adapter.rs @@ -935,6 +935,7 @@ impl HardwareAdapter { z: 2.0, sensor_type: SensorType::Transmitter, is_operational: true, + last_rssi: Some(-42.0), }, status: SensorStatus::Connected, last_rssi: Some(-42.0), @@ -951,6 +952,7 @@ impl HardwareAdapter { z: 2.0, sensor_type: SensorType::Receiver, is_operational: true, + last_rssi: Some(-48.0), }, status: SensorStatus::Connected, last_rssi: Some(-48.0), @@ -1293,6 +1295,7 @@ mod tests { z: 1.5, sensor_type: SensorType::Transceiver, is_operational: true, + last_rssi: Some(-45.0), }, status: SensorStatus::Connected, last_rssi: Some(-45.0), diff --git a/v2/crates/wifi-densepose-mat/src/localization/fusion.rs b/v2/crates/wifi-densepose-mat/src/localization/fusion.rs index df418b69..3bf0ea9a 100644 --- a/v2/crates/wifi-densepose-mat/src/localization/fusion.rs +++ b/v2/crates/wifi-densepose-mat/src/localization/fusion.rs @@ -35,12 +35,23 @@ impl LocalizationService { } } - /// Estimate survivor position + /// Estimate survivor position from real per-sensor RSSI + debris-aware depth. + /// + /// `vitals` is currently used only as a presence guard (position is only + /// meaningful for a real detection) — the position itself is derived from + /// sensor geometry + RSSI and the zone debris profile, not from the vital + /// waveform. It is retained in the signature so depth weighting can later + /// incorporate breathing-amplitude SNR without a breaking API change. pub fn estimate_position( &self, vitals: &VitalSignsReading, zone: &ScanZone, ) -> Option { + // Only attempt localization for a real detection. + if !vitals.has_vitals() { + return None; + } + // Get sensor positions let sensors = zone.sensor_positions(); @@ -48,9 +59,13 @@ impl LocalizationService { return None; } - // Estimate 2D position from triangulation - // In real implementation, RSSI values would come from actual measurements - let rssi_values = self.simulate_rssi_measurements(sensors, vitals); + // Estimate 2D position from triangulation using REAL per-sensor RSSI. + // Sensors that have no live RSSI reading contribute nothing — we never + // fabricate a measurement. If fewer than the triangulator's minimum + // report real RSSI, `estimate_position` returns None and the caller + // records the survivor with `location: None` (dedup then falls back to + // the zone + vitals-signature path rather than inflating the count). + let rssi_values = self.collect_rssi_measurements(sensors); let position_2d = self.triangulator.estimate_position(sensors, &rssi_values)?; // Estimate depth @@ -71,21 +86,35 @@ impl LocalizationService { Some(position_3d) } - /// Read RSSI measurements from sensors. + /// Collect REAL per-sensor RSSI measurements for triangulation. /// - /// Returns empty when no real sensor hardware is connected. - /// Real RSSI readings require ESP32 mesh (ADR-012) or Linux WiFi interface (ADR-013). - /// Caller handles empty readings by returning None/default. - fn simulate_rssi_measurements( + /// Reads each operational sensor's most recent live RSSI (`last_rssi`, + /// populated by the hardware layer from actual signal-strength readings). + /// Sensors without a real reading are omitted — no value is fabricated. When + /// the number of real measurements is below the triangulator's minimum the + /// returned vector is short and `Triangulator::estimate_position` yields + /// `None`, so the survivor is recorded with no location and de-duplicated by + /// vitals signature instead of being counted multiple times. + fn collect_rssi_measurements( &self, - _sensors: &[crate::domain::SensorPosition], - _vitals: &VitalSignsReading, + sensors: &[crate::domain::SensorPosition], ) -> Vec<(String, f64)> { - // No real sensor hardware connected - return empty. - // Real RSSI readings require ESP32 mesh (ADR-012) or Linux WiFi interface (ADR-013). - // Caller handles empty readings by returning None from estimate_position. - tracing::warn!("No sensor hardware connected. Real RSSI readings require ESP32 mesh (ADR-012) or Linux WiFi interface (ADR-013)."); - vec![] + let measurements: Vec<(String, f64)> = sensors + .iter() + .filter(|s| s.is_operational) + .filter_map(|s| s.last_rssi.map(|rssi| (s.id.clone(), rssi))) + .collect(); + + if measurements.len() < self.triangulator.config().min_sensors { + tracing::debug!( + real_rssi_count = measurements.len(), + required = self.triangulator.config().min_sensors, + "Insufficient real RSSI measurements for triangulation; \ + survivor will be recorded without a fixed location (no RSSI fabricated)." + ); + } + + measurements } /// Estimate debris profile for the zone @@ -382,4 +411,84 @@ mod tests { // Just verify it creates without panic drop(service); } + + /// Real-RSSI localization: when ≥3 sensors carry live RSSI the service + /// produces a position (exercises the real triangulator path, replacing the + /// old `simulate_rssi_measurements` that always returned `vec![]`). + #[test] + fn test_estimate_position_uses_real_rssi() { + use crate::domain::{ + BreathingPattern, BreathingType, MovementProfile, ScanZone, SensorPosition, SensorType, + VitalSignsReading, ZoneBounds, + }; + + let mut zone = ScanZone::new("Z", ZoneBounds::rectangle(0.0, 0.0, 12.0, 12.0)); + for (id, x, y, rssi) in [ + ("s1", 0.0, 0.0, -55.0), + ("s2", 10.0, 0.0, -60.0), + ("s3", 5.0, 10.0, -58.0), + ] { + zone.add_sensor(SensorPosition { + id: id.to_string(), + x, + y, + z: 1.5, + sensor_type: SensorType::Transceiver, + is_operational: true, + last_rssi: Some(rssi), + }); + } + + let vitals = VitalSignsReading::new( + Some(BreathingPattern { + rate_bpm: 16.0, + amplitude: 0.8, + regularity: 0.9, + pattern_type: BreathingType::Normal, + }), + None, + MovementProfile::default(), + ); + + let service = LocalizationService::new(); + let pos = service.estimate_position(&vitals, &zone); + assert!(pos.is_some(), "3 real RSSI sensors should yield a position"); + } + + /// Honest negative: sensors WITHOUT real RSSI yield no position (no + /// fabrication). The caller then records `location: None`. + #[test] + fn test_estimate_position_none_without_real_rssi() { + use crate::domain::{ + BreathingPattern, BreathingType, MovementProfile, ScanZone, SensorPosition, SensorType, + VitalSignsReading, ZoneBounds, + }; + + let mut zone = ScanZone::new("Z", ZoneBounds::rectangle(0.0, 0.0, 12.0, 12.0)); + for (id, x, y) in [("s1", 0.0, 0.0), ("s2", 10.0, 0.0), ("s3", 5.0, 10.0)] { + zone.add_sensor(SensorPosition { + id: id.to_string(), + x, + y, + z: 1.5, + sensor_type: SensorType::Transceiver, + is_operational: true, + last_rssi: None, // no live signal + }); + } + + let vitals = VitalSignsReading::new( + Some(BreathingPattern { + rate_bpm: 16.0, + amplitude: 0.8, + regularity: 0.9, + pattern_type: BreathingType::Normal, + }), + None, + MovementProfile::default(), + ); + + let service = LocalizationService::new(); + assert!(service.estimate_position(&vitals, &zone).is_none()); + } } diff --git a/v2/crates/wifi-densepose-mat/src/localization/triangulation.rs b/v2/crates/wifi-densepose-mat/src/localization/triangulation.rs index 52de7f90..9dab09ac 100644 --- a/v2/crates/wifi-densepose-mat/src/localization/triangulation.rs +++ b/v2/crates/wifi-densepose-mat/src/localization/triangulation.rs @@ -60,6 +60,11 @@ impl Triangulator { Self::new(TriangulationConfig::default()) } + /// Access the triangulation configuration. + pub fn config(&self) -> &TriangulationConfig { + &self.config + } + /// Estimate position from RSSI measurements pub fn estimate_position( &self, @@ -300,6 +305,7 @@ mod tests { z: 1.5, sensor_type: SensorType::Transceiver, is_operational: true, + last_rssi: None, }, SensorPosition { id: "s2".to_string(), @@ -308,6 +314,7 @@ mod tests { z: 1.5, sensor_type: SensorType::Transceiver, is_operational: true, + last_rssi: None, }, SensorPosition { id: "s3".to_string(), @@ -316,6 +323,7 @@ mod tests { z: 1.5, sensor_type: SensorType::Transceiver, is_operational: true, + last_rssi: None, }, ] }