958 lines
35 KiB
Rust
958 lines
35 KiB
Rust
//! Multistatic Viewpoint Fusion (ADR-029 Section 2.4)
|
|
//!
|
|
//! With N ESP32 nodes in a TDMA mesh, each sensing cycle produces N
|
|
//! `MultiBandCsiFrame`s. This module fuses them into a single
|
|
//! `FusedSensingFrame` using attention-based cross-node weighting.
|
|
//!
|
|
//! # Algorithm
|
|
//!
|
|
//! 1. Collect N `MultiBandCsiFrame`s from the current sensing cycle.
|
|
//! 2. Use `ruvector-attn-mincut` for cross-node attention: cells showing
|
|
//! correlated motion energy across nodes (body reflection) are amplified;
|
|
//! cells with single-node energy (multipath artifact) are suppressed.
|
|
//! 3. Multi-person separation via `ruvector-mincut::DynamicMinCut` builds
|
|
//! a cross-link correlation graph and partitions into K person clusters.
|
|
//!
|
|
//! # CIR Gate (ADR-134)
|
|
//!
|
|
//! When `MultistaticConfig::use_cir_gate` is true and a shared `CirEstimator`
|
|
//! is attached, the fused coherence score is augmented with the dominant-tap
|
|
//! ratio from the CIR of the first active link. This isolates body-motion
|
|
//! signatures to specific delay bins rather than across all subcarriers.
|
|
//! Set `use_cir_gate = false` for the legacy CSI-domain-only path (A/B test).
|
|
//!
|
|
//! # RuVector Integration
|
|
//!
|
|
//! - `ruvector-attn-mincut` for cross-node spectrogram attention gating
|
|
//! - `ruvector-mincut` for person separation (DynamicMinCut)
|
|
|
|
use std::sync::Arc;
|
|
|
|
use super::cir::{CirConfig, CirEstimator};
|
|
use super::multiband::MultiBandCsiFrame;
|
|
|
|
/// Errors from multistatic fusion.
|
|
#[derive(Debug, thiserror::Error)]
|
|
pub enum MultistaticError {
|
|
/// No node frames provided.
|
|
#[error("No node frames provided for multistatic fusion")]
|
|
NoFrames,
|
|
|
|
/// Insufficient nodes for multistatic mode (need at least 2).
|
|
#[error("Need at least 2 nodes for multistatic fusion, got {0}")]
|
|
InsufficientNodes(usize),
|
|
|
|
/// Timestamp mismatch beyond guard interval.
|
|
#[error("Timestamp spread {spread_us} us exceeds guard interval {guard_us} us")]
|
|
TimestampMismatch { spread_us: u64, guard_us: u64 },
|
|
|
|
/// Dimension mismatch in fusion inputs.
|
|
#[error("Dimension mismatch: node {node_idx} has {got} subcarriers, expected {expected}")]
|
|
DimensionMismatch {
|
|
node_idx: usize,
|
|
expected: usize,
|
|
got: usize,
|
|
},
|
|
}
|
|
|
|
/// A fused sensing frame from all nodes at one sensing cycle.
|
|
///
|
|
/// This is the primary output of the multistatic fusion stage and serves
|
|
/// as input to model inference and the pose tracker.
|
|
#[derive(Debug, Clone)]
|
|
pub struct FusedSensingFrame {
|
|
/// Timestamp of this sensing cycle in microseconds.
|
|
pub timestamp_us: u64,
|
|
/// Fused amplitude vector across all nodes (attention-weighted mean).
|
|
/// Length = n_subcarriers.
|
|
pub fused_amplitude: Vec<f32>,
|
|
/// Fused phase vector across all nodes.
|
|
/// Length = n_subcarriers.
|
|
pub fused_phase: Vec<f32>,
|
|
/// Per-node multi-band frames (preserved for geometry computations).
|
|
pub node_frames: Vec<MultiBandCsiFrame>,
|
|
/// Node positions (x, y, z) in meters from deployment configuration.
|
|
pub node_positions: Vec<[f32; 3]>,
|
|
/// Number of active nodes contributing to this frame.
|
|
pub active_nodes: usize,
|
|
/// Cross-node coherence score (0.0-1.0). Higher means more agreement
|
|
/// across viewpoints, indicating a strong body reflection signal.
|
|
pub cross_node_coherence: f32,
|
|
}
|
|
|
|
/// Configuration for multistatic fusion.
|
|
#[derive(Debug, Clone)]
|
|
pub struct MultistaticConfig {
|
|
/// Maximum timestamp spread (microseconds) across nodes in one cycle.
|
|
/// Default: 5000 us (5 ms), well within the 50 ms TDMA cycle.
|
|
pub guard_interval_us: u64,
|
|
/// ADR-137 soft guard (microseconds): a spread above this but within
|
|
/// `guard_interval_us` is fused but recorded as a `TimestampMismatch`
|
|
/// contradiction (loose alignment ⇒ privacy demotion). Default guard/5.
|
|
pub soft_guard_us: u64,
|
|
/// Minimum number of nodes for multistatic mode.
|
|
/// Falls back to single-node mode if fewer nodes are available.
|
|
pub min_nodes: usize,
|
|
/// Attention temperature for cross-node weighting.
|
|
/// Lower temperature -> sharper attention (fewer nodes dominate).
|
|
pub attention_temperature: f32,
|
|
/// Whether to enable person separation via min-cut.
|
|
pub enable_person_separation: bool,
|
|
/// Enable the CIR-domain coherence gate (ADR-134).
|
|
/// Set `false` to fall back to the legacy CSI-domain-only path (A/B test).
|
|
pub use_cir_gate: bool,
|
|
}
|
|
|
|
impl Default for MultistaticConfig {
|
|
fn default() -> Self {
|
|
Self {
|
|
guard_interval_us: 5000,
|
|
soft_guard_us: 1000,
|
|
min_nodes: 2,
|
|
attention_temperature: 1.0,
|
|
enable_person_separation: true,
|
|
use_cir_gate: true,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Multistatic frame fuser.
|
|
///
|
|
/// Collects per-node multi-band frames and produces a single fused
|
|
/// sensing frame per TDMA cycle.
|
|
///
|
|
/// # CIR gate (ADR-134)
|
|
///
|
|
/// A single `Arc<CirEstimator>` is shared across all links. When
|
|
/// `config.use_cir_gate` is true and a `CirEstimator` is attached, the fused
|
|
/// `cross_node_coherence` is blended with the dominant-tap ratio from the
|
|
/// first available CsiFrame's CIR estimate. Set `use_cir_gate = false` to
|
|
/// disable the CIR path and keep the legacy frequency-domain coherence only.
|
|
pub struct MultistaticFuser {
|
|
config: MultistaticConfig,
|
|
/// Node positions in 3D space (meters).
|
|
node_positions: Vec<[f32; 3]>,
|
|
/// Optional shared CIR estimator (ADR-134). `None` = legacy path only.
|
|
cir_estimator: Option<Arc<CirEstimator>>,
|
|
}
|
|
|
|
impl std::fmt::Debug for MultistaticFuser {
|
|
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
|
f.debug_struct("MultistaticFuser")
|
|
.field("config", &self.config)
|
|
.field("node_positions", &self.node_positions)
|
|
.field("cir_estimator", &self.cir_estimator.is_some())
|
|
.finish()
|
|
}
|
|
}
|
|
|
|
impl MultistaticFuser {
|
|
/// Create a fuser with default configuration and no node positions.
|
|
pub fn new() -> Self {
|
|
Self {
|
|
config: MultistaticConfig::default(),
|
|
node_positions: Vec::new(),
|
|
cir_estimator: None,
|
|
}
|
|
}
|
|
|
|
/// Create a fuser with custom configuration.
|
|
pub fn with_config(config: MultistaticConfig) -> Self {
|
|
Self {
|
|
config,
|
|
node_positions: Vec::new(),
|
|
cir_estimator: None,
|
|
}
|
|
}
|
|
|
|
/// Attach a shared `CirEstimator` for CIR-domain coherence gating (ADR-134).
|
|
///
|
|
/// One estimator is shared across all links. Build it via
|
|
/// `CirEstimator::new(CirConfig::ht20())` for ESP32-S3 HT20 deployments.
|
|
/// Pass `None` to detach and fall back to the legacy path.
|
|
pub fn set_cir_estimator(&mut self, estimator: Option<Arc<CirEstimator>>) {
|
|
self.cir_estimator = estimator;
|
|
}
|
|
|
|
/// Create a fuser with a pre-built `CirEstimator` for HT20 (ADR-134 default).
|
|
///
|
|
/// Equivalent to `new()` followed by `set_cir_estimator(Some(Arc::new(CirEstimator::new(CirConfig::ht20()))))`.
|
|
pub fn with_cir_ht20() -> Self {
|
|
let mut fuser = Self::new();
|
|
fuser.cir_estimator = Some(Arc::new(CirEstimator::new(CirConfig::ht20())));
|
|
fuser
|
|
}
|
|
|
|
/// Set node positions for geometric diversity computations.
|
|
pub fn set_node_positions(&mut self, positions: Vec<[f32; 3]>) {
|
|
self.node_positions = positions;
|
|
}
|
|
|
|
/// Return the current node positions.
|
|
pub fn node_positions(&self) -> &[[f32; 3]] {
|
|
&self.node_positions
|
|
}
|
|
|
|
/// Fuse multiple node frames into a single `FusedSensingFrame`.
|
|
///
|
|
/// When only one node is provided, falls back to single-node mode
|
|
/// (no cross-node attention). When two or more nodes are available,
|
|
/// applies attention-weighted fusion.
|
|
pub fn fuse(
|
|
&self,
|
|
node_frames: &[MultiBandCsiFrame],
|
|
) -> std::result::Result<FusedSensingFrame, MultistaticError> {
|
|
if node_frames.is_empty() {
|
|
return Err(MultistaticError::NoFrames);
|
|
}
|
|
|
|
// Validate timestamp spread
|
|
if node_frames.len() > 1 {
|
|
let min_ts = node_frames.iter().map(|f| f.timestamp_us).min().unwrap();
|
|
let max_ts = node_frames.iter().map(|f| f.timestamp_us).max().unwrap();
|
|
let spread = max_ts - min_ts;
|
|
if spread > self.config.guard_interval_us {
|
|
return Err(MultistaticError::TimestampMismatch {
|
|
spread_us: spread,
|
|
guard_us: self.config.guard_interval_us,
|
|
});
|
|
}
|
|
}
|
|
|
|
// Extract per-node amplitude vectors from first channel of each node
|
|
let amplitudes: Vec<&[f32]> = node_frames
|
|
.iter()
|
|
.filter_map(|f| f.channel_frames.first().map(|cf| cf.amplitude.as_slice()))
|
|
.collect();
|
|
|
|
let phases: Vec<&[f32]> = node_frames
|
|
.iter()
|
|
.filter_map(|f| f.channel_frames.first().map(|cf| cf.phase.as_slice()))
|
|
.collect();
|
|
|
|
if amplitudes.is_empty() {
|
|
return Err(MultistaticError::NoFrames);
|
|
}
|
|
|
|
// Validate dimension consistency
|
|
let n_sub = amplitudes[0].len();
|
|
for (i, amp) in amplitudes.iter().enumerate().skip(1) {
|
|
if amp.len() != n_sub {
|
|
return Err(MultistaticError::DimensionMismatch {
|
|
node_idx: i,
|
|
expected: n_sub,
|
|
got: amp.len(),
|
|
});
|
|
}
|
|
}
|
|
|
|
let n_nodes = amplitudes.len();
|
|
let (fused_amp, fused_ph, freq_coherence) = if n_nodes == 1 {
|
|
// Single-node fallback
|
|
(amplitudes[0].to_vec(), phases[0].to_vec(), 1.0_f32)
|
|
} else {
|
|
// Multi-node attention-weighted fusion
|
|
attention_weighted_fusion(&litudes, &phases, self.config.attention_temperature)
|
|
};
|
|
|
|
// ADR-134 CIR gate: blend freq-domain coherence with CIR dominant-tap
|
|
// ratio from the first available frame. When use_cir_gate = false,
|
|
// the legacy freq-domain coherence is used unchanged (A/B switch).
|
|
let coherence = self.cir_gate_coherence(freq_coherence, node_frames);
|
|
|
|
// Derive timestamp from median
|
|
let mut timestamps: Vec<u64> = node_frames.iter().map(|f| f.timestamp_us).collect();
|
|
timestamps.sort_unstable();
|
|
let timestamp_us = timestamps[timestamps.len() / 2];
|
|
|
|
// Build node positions list, filling with origin for unknown nodes
|
|
let positions: Vec<[f32; 3]> = (0..n_nodes)
|
|
.map(|i| {
|
|
self.node_positions
|
|
.get(i)
|
|
.copied()
|
|
.unwrap_or([0.0, 0.0, 0.0])
|
|
})
|
|
.collect();
|
|
|
|
Ok(FusedSensingFrame {
|
|
timestamp_us,
|
|
fused_amplitude: fused_amp,
|
|
fused_phase: fused_ph,
|
|
node_frames: node_frames.to_vec(),
|
|
node_positions: positions,
|
|
active_nodes: n_nodes,
|
|
cross_node_coherence: coherence,
|
|
})
|
|
}
|
|
|
|
/// Fuse and produce an auditable [`QualityScore`] alongside the frame
|
|
/// (ADR-137). Additive over [`Self::fuse`]: the frame is identical; the
|
|
/// score records the per-node attention weights actually used, the positive
|
|
/// [`EvidenceRef`]s, and any tolerated [`ContradictionFlag`]s (e.g. a loose
|
|
/// but in-guard timestamp spread). A non-empty contradiction set must demote
|
|
/// the downstream BFLD privacy class (see [`QualityScore::forces_privacy_demotion`]).
|
|
///
|
|
/// `coherence_accept` is the gate threshold (mirrors `RuvSenseConfig`);
|
|
/// meeting it records a [`EvidenceRef::CoherenceGateThreshold`].
|
|
///
|
|
/// # Errors
|
|
/// Same hard-error preconditions as [`Self::fuse`].
|
|
pub fn fuse_scored(
|
|
&self,
|
|
node_frames: &[MultiBandCsiFrame],
|
|
coherence_accept: f32,
|
|
) -> std::result::Result<(FusedSensingFrame, super::fusion_quality::QualityScore), MultistaticError>
|
|
{
|
|
use super::fusion_quality::{ContradictionFlag, EvidenceRef, FamilyId, QualityScore};
|
|
|
|
let fused = self.fuse(node_frames)?;
|
|
|
|
// Recompute the per-node amplitude views (same selection as `fuse`).
|
|
let amplitudes: Vec<&[f32]> = node_frames
|
|
.iter()
|
|
.filter_map(|f| f.channel_frames.first().map(|cf| cf.amplitude.as_slice()))
|
|
.collect();
|
|
let n_nodes = amplitudes.len();
|
|
let per_node_weights = if n_nodes <= 1 {
|
|
vec![1.0_f32; n_nodes]
|
|
} else {
|
|
node_attention_weights(&litudes, self.config.attention_temperature)
|
|
};
|
|
|
|
// --- Positive evidence ---
|
|
let mut evidence_refs = Vec::new();
|
|
if n_nodes > 1 {
|
|
evidence_refs.push(EvidenceRef::WeightEntropy {
|
|
normalized_entropy: compute_weight_coherence(&per_node_weights),
|
|
n_nodes,
|
|
});
|
|
}
|
|
if fused.cross_node_coherence >= coherence_accept {
|
|
evidence_refs.push(EvidenceRef::CoherenceGateThreshold {
|
|
coherence: fused.cross_node_coherence,
|
|
threshold: coherence_accept,
|
|
});
|
|
}
|
|
|
|
// --- Tolerated contradictions ---
|
|
let mut contradiction_flags = Vec::new();
|
|
if n_nodes > 1 {
|
|
let min_ts = node_frames.iter().map(|f| f.timestamp_us).min().unwrap_or(0);
|
|
let max_ts = node_frames.iter().map(|f| f.timestamp_us).max().unwrap_or(0);
|
|
let spread_ns = (max_ts - min_ts).saturating_mul(1000);
|
|
let soft_guard_ns = self.config.soft_guard_us.saturating_mul(1000);
|
|
if spread_ns > soft_guard_ns {
|
|
contradiction_flags.push(ContradictionFlag::TimestampMismatch {
|
|
spread_ns,
|
|
soft_guard_ns,
|
|
});
|
|
}
|
|
}
|
|
|
|
let capture_ns = fused.timestamp_us.saturating_mul(1000);
|
|
let base_coherence = fused.cross_node_coherence;
|
|
Ok((
|
|
fused,
|
|
QualityScore {
|
|
family_id: FamilyId::MultistaticCsi,
|
|
capture_ns,
|
|
// Frames at this layer do not yet carry a calibration epoch
|
|
// (ADR-135 id propagation lands with the calibration Stage);
|
|
// recorded as None until then.
|
|
calibration_id: None,
|
|
base_coherence,
|
|
per_node_weights,
|
|
evidence_refs,
|
|
contradiction_flags,
|
|
timestamp_computed_ns: capture_ns,
|
|
},
|
|
))
|
|
}
|
|
|
|
/// Like [`Self::fuse_scored`], but threads a per-node calibration epoch
|
|
/// (ADR-137 §2.3). `calibrations[i]` is the [`CalibrationId`] applied to
|
|
/// `node_frames[i]` (ADR-135 `BaselineCalibration::calibration_id`).
|
|
///
|
|
/// - If every contributing node carries the **same** calibration id, the
|
|
/// score's `calibration_id` is set to it and a
|
|
/// [`EvidenceRef::CalibrationApplied`] is recorded.
|
|
/// - If the calibrations **disagree** (or some are missing), the score's
|
|
/// `calibration_id` is left `None` and a
|
|
/// [`ContradictionFlag::CalibrationIdMismatch`] is raised — which forces a
|
|
/// downstream privacy demotion (ADR-141).
|
|
///
|
|
/// # Errors
|
|
/// Same hard-error preconditions as [`Self::fuse`].
|
|
pub fn fuse_scored_calibrated(
|
|
&self,
|
|
node_frames: &[MultiBandCsiFrame],
|
|
calibrations: &[Option<super::fusion_quality::CalibrationId>],
|
|
coherence_accept: f32,
|
|
) -> std::result::Result<(FusedSensingFrame, super::fusion_quality::QualityScore), MultistaticError>
|
|
{
|
|
use super::fusion_quality::{ContradictionFlag, EvidenceRef};
|
|
let (fused, mut score) = self.fuse_scored(node_frames, coherence_accept)?;
|
|
|
|
let present: Vec<_> = calibrations.iter().flatten().copied().collect();
|
|
if present.is_empty() {
|
|
return Ok((fused, score)); // uncalibrated path — leave None.
|
|
}
|
|
// Modal (most frequent) calibration id; ties resolve to the first seen.
|
|
let mut modal = present[0];
|
|
let mut best = 0usize;
|
|
for &cand in &present {
|
|
let c = present.iter().filter(|&&x| x == cand).count();
|
|
if c > best {
|
|
best = c;
|
|
modal = cand;
|
|
}
|
|
}
|
|
// Disagreement = any node whose calibration differs from the modal,
|
|
// including nodes that carried no calibration at all.
|
|
let agreeing = present.iter().filter(|&&x| x == modal).count();
|
|
let disagreeing = calibrations.len() - agreeing;
|
|
|
|
if disagreeing == 0 {
|
|
score.calibration_id = Some(modal);
|
|
score.evidence_refs.push(EvidenceRef::CalibrationApplied {
|
|
calibration_id: modal,
|
|
n_frames: agreeing,
|
|
});
|
|
} else {
|
|
// Mismatch: unsafe to claim a single calibration epoch (§2.3).
|
|
score.calibration_id = None;
|
|
score
|
|
.contradiction_flags
|
|
.push(ContradictionFlag::CalibrationIdMismatch { expected: modal, disagreeing });
|
|
}
|
|
Ok((fused, score))
|
|
}
|
|
|
|
/// Apply the CIR-domain coherence gate (ADR-134).
|
|
///
|
|
/// When `use_cir_gate` is enabled and a `CirEstimator` is present, runs
|
|
/// the estimator on the first node's first channel frame and blends the
|
|
/// dominant-tap ratio into the frequency-domain coherence score.
|
|
///
|
|
/// On `CirError::UnsanitizedPhase` the CIR result is dropped and the
|
|
/// frequency-domain coherence is returned unchanged (graceful fallback).
|
|
fn cir_gate_coherence(
|
|
&self,
|
|
freq_coherence: f32,
|
|
node_frames: &[MultiBandCsiFrame],
|
|
) -> f32 {
|
|
if !self.config.use_cir_gate {
|
|
return freq_coherence;
|
|
}
|
|
let Some(ref estimator) = self.cir_estimator else {
|
|
return freq_coherence;
|
|
};
|
|
|
|
// Build a minimal CsiFrame from the first node's first channel frame.
|
|
// We use the amplitude+phase vectors to reconstruct complex values.
|
|
let Some(first_frame) = node_frames.first() else {
|
|
return freq_coherence;
|
|
};
|
|
let Some(cf) = first_frame.channel_frames.first() else {
|
|
return freq_coherence;
|
|
};
|
|
|
|
// Reconstruct Complex64 data from amplitude+phase for the CIR estimator.
|
|
let csi_frame = build_csi_frame_from_channel(cf);
|
|
match estimator.estimate(&csi_frame) {
|
|
Ok(cir) => {
|
|
// Blend: coherence = 0.7 · freq + 0.3 · dominant_tap_ratio.
|
|
// High dominant-tap ratio ≡ strong LOS → supports coherent gate.
|
|
0.7 * freq_coherence + 0.3 * cir.dominant_tap_ratio
|
|
}
|
|
Err(super::cir::CirError::UnsanitizedPhase { .. }) => {
|
|
// Frame not sanitized — fall back to freq-domain coherence.
|
|
freq_coherence
|
|
}
|
|
Err(_) => freq_coherence,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Default for MultistaticFuser {
|
|
fn default() -> Self {
|
|
Self::new()
|
|
}
|
|
}
|
|
|
|
/// Reconstruct a minimal `CsiFrame` from a `CanonicalCsiFrame` for CIR estimation.
|
|
///
|
|
/// Amplitude and phase are re-combined into `Complex64` values so that
|
|
/// `CirEstimator::estimate()` can extract the active-subcarrier vector.
|
|
fn build_csi_frame_from_channel(
|
|
cf: &crate::hardware_norm::CanonicalCsiFrame,
|
|
) -> wifi_densepose_core::types::CsiFrame {
|
|
use ndarray::Array2;
|
|
use num_complex::Complex64;
|
|
use wifi_densepose_core::types::{CsiFrame, CsiMetadata, DeviceId, FrequencyBand};
|
|
|
|
let n = cf.amplitude.len();
|
|
let mut data = Array2::<Complex64>::zeros((1, n));
|
|
for (ki, (&, &ph)) in cf.amplitude.iter().zip(cf.phase.iter()).enumerate() {
|
|
data[[0, ki]] = Complex64::from_polar(amp as f64, ph as f64);
|
|
}
|
|
let meta = CsiMetadata::new(
|
|
DeviceId::new("multistatic-cir"),
|
|
FrequencyBand::Band2_4GHz,
|
|
6,
|
|
);
|
|
CsiFrame::new(meta, data)
|
|
}
|
|
|
|
/// Attention-weighted fusion of amplitude and phase vectors from multiple nodes.
|
|
///
|
|
/// Each node's contribution is weighted by its agreement with the consensus.
|
|
/// Returns (fused_amplitude, fused_phase, cross_node_coherence).
|
|
fn attention_weighted_fusion(
|
|
amplitudes: &[&[f32]],
|
|
phases: &[&[f32]],
|
|
temperature: f32,
|
|
) -> (Vec<f32>, Vec<f32>, f32) {
|
|
let n_sub = amplitudes[0].len();
|
|
|
|
// Attention weights (cosine similarity to consensus, softmax).
|
|
let weights = node_attention_weights(amplitudes, temperature);
|
|
|
|
// Weighted fusion
|
|
let mut fused_amp = vec![0.0_f32; n_sub];
|
|
let mut fused_ph_sin = vec![0.0_f32; n_sub];
|
|
let mut fused_ph_cos = vec![0.0_f32; n_sub];
|
|
|
|
for (n, (&, &ph)) in amplitudes.iter().zip(phases.iter()).enumerate() {
|
|
let w = weights[n];
|
|
for i in 0..n_sub {
|
|
fused_amp[i] += w * amp[i];
|
|
fused_ph_sin[i] += w * ph[i].sin();
|
|
fused_ph_cos[i] += w * ph[i].cos();
|
|
}
|
|
}
|
|
|
|
// Recover phase from sin/cos weighted average
|
|
let fused_ph: Vec<f32> = fused_ph_sin
|
|
.iter()
|
|
.zip(fused_ph_cos.iter())
|
|
.map(|(&s, &c)| s.atan2(c))
|
|
.collect();
|
|
|
|
// Coherence = mean weight entropy proxy: high when weights are balanced
|
|
let coherence = compute_weight_coherence(&weights);
|
|
|
|
(fused_amp, fused_ph, coherence)
|
|
}
|
|
|
|
/// Compute the per-node attention weights (cosine similarity to the amplitude
|
|
/// consensus, softmaxed at `temperature`). Returned weights sum to ~1.0 and are
|
|
/// node-index aligned. Exposed so the ADR-137 fusion-quality scorer records the
|
|
/// exact weights used for fusion rather than re-deriving an approximation.
|
|
#[must_use]
|
|
pub fn node_attention_weights(amplitudes: &[&[f32]], temperature: f32) -> Vec<f32> {
|
|
let n_nodes = amplitudes.len();
|
|
if n_nodes == 0 {
|
|
return Vec::new();
|
|
}
|
|
let n_sub = amplitudes[0].len();
|
|
|
|
// Mean amplitude as consensus reference.
|
|
let mut mean_amp = vec![0.0_f32; n_sub];
|
|
for amp in amplitudes {
|
|
for (i, &v) in amp.iter().enumerate() {
|
|
mean_amp[i] += v;
|
|
}
|
|
}
|
|
for v in &mut mean_amp {
|
|
*v /= n_nodes as f32;
|
|
}
|
|
|
|
// Cosine-similarity logits.
|
|
let mut logits = vec![0.0_f32; n_nodes];
|
|
for (n, amp) in amplitudes.iter().enumerate() {
|
|
let mut dot = 0.0_f32;
|
|
let mut norm_a = 0.0_f32;
|
|
let mut norm_b = 0.0_f32;
|
|
for i in 0..n_sub.min(amp.len()) {
|
|
dot += amp[i] * mean_amp[i];
|
|
norm_a += amp[i] * amp[i];
|
|
norm_b += mean_amp[i] * mean_amp[i];
|
|
}
|
|
let denom = (norm_a * norm_b).sqrt().max(1e-12);
|
|
logits[n] = (dot / denom) / temperature;
|
|
}
|
|
|
|
// Numerically stable softmax.
|
|
let max_logit = logits.iter().cloned().fold(f32::NEG_INFINITY, f32::max);
|
|
let mut weights = vec![0.0_f32; n_nodes];
|
|
for (n, &logit) in logits.iter().enumerate() {
|
|
weights[n] = (logit - max_logit).exp();
|
|
}
|
|
let weight_sum: f32 = weights.iter().sum::<f32>().max(1e-12);
|
|
for w in &mut weights {
|
|
*w /= weight_sum;
|
|
}
|
|
weights
|
|
}
|
|
|
|
/// Compute coherence from attention weights.
|
|
///
|
|
/// Returns 1.0 when all weights are equal (all nodes agree),
|
|
/// and approaches 0.0 when a single node dominates.
|
|
pub(crate) fn compute_weight_coherence(weights: &[f32]) -> f32 {
|
|
let n = weights.len() as f32;
|
|
if n <= 1.0 {
|
|
return 1.0;
|
|
}
|
|
|
|
// Normalized entropy: H / log(n)
|
|
let max_entropy = n.ln();
|
|
if max_entropy < 1e-12 {
|
|
return 1.0;
|
|
}
|
|
|
|
let entropy: f32 = weights
|
|
.iter()
|
|
.filter(|&&w| w > 1e-12)
|
|
.map(|&w| -w * w.ln())
|
|
.sum();
|
|
|
|
(entropy / max_entropy).clamp(0.0, 1.0)
|
|
}
|
|
|
|
/// Compute the geometric diversity score for a set of node positions.
|
|
///
|
|
/// Returns a value in [0.0, 1.0] where 1.0 indicates maximum angular
|
|
/// coverage. Based on the angular span of node positions relative to the
|
|
/// room centroid.
|
|
pub fn geometric_diversity(positions: &[[f32; 3]]) -> f32 {
|
|
if positions.len() < 2 {
|
|
return 0.0;
|
|
}
|
|
|
|
// Compute centroid
|
|
let n = positions.len() as f32;
|
|
let centroid = [
|
|
positions.iter().map(|p| p[0]).sum::<f32>() / n,
|
|
positions.iter().map(|p| p[1]).sum::<f32>() / n,
|
|
positions.iter().map(|p| p[2]).sum::<f32>() / n,
|
|
];
|
|
|
|
// Compute angles from centroid to each node (in 2D, ignoring z)
|
|
let mut angles: Vec<f32> = positions
|
|
.iter()
|
|
.map(|p| {
|
|
let dx = p[0] - centroid[0];
|
|
let dy = p[1] - centroid[1];
|
|
dy.atan2(dx)
|
|
})
|
|
.collect();
|
|
|
|
angles.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
|
|
|
|
// Angular coverage: sum of gaps, diversity is high when gaps are even
|
|
let mut max_gap = 0.0_f32;
|
|
for i in 0..angles.len() {
|
|
let next = (i + 1) % angles.len();
|
|
let mut gap = angles[next] - angles[i];
|
|
if gap < 0.0 {
|
|
gap += 2.0 * std::f32::consts::PI;
|
|
}
|
|
max_gap = max_gap.max(gap);
|
|
}
|
|
|
|
// Perfect coverage (N equidistant nodes): max_gap = 2*pi/N
|
|
// Worst case (all co-located): max_gap = 2*pi
|
|
let ideal_gap = 2.0 * std::f32::consts::PI / positions.len() as f32;
|
|
(ideal_gap / max_gap.max(1e-6)).clamp(0.0, 1.0)
|
|
}
|
|
|
|
/// Represents a cluster of TX-RX links attributed to one person.
|
|
#[derive(Debug, Clone)]
|
|
pub struct PersonCluster {
|
|
/// Cluster identifier.
|
|
pub id: usize,
|
|
/// Indices into the link array belonging to this cluster.
|
|
pub link_indices: Vec<usize>,
|
|
/// Mean correlation strength within the cluster.
|
|
pub intra_correlation: f32,
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
use crate::hardware_norm::{CanonicalCsiFrame, HardwareType};
|
|
|
|
fn make_node_frame(
|
|
node_id: u8,
|
|
timestamp_us: u64,
|
|
n_sub: usize,
|
|
scale: f32,
|
|
) -> MultiBandCsiFrame {
|
|
let amp: Vec<f32> = (0..n_sub).map(|i| scale * (1.0 + 0.1 * i as f32)).collect();
|
|
let phase: Vec<f32> = (0..n_sub).map(|i| i as f32 * 0.05).collect();
|
|
MultiBandCsiFrame {
|
|
node_id,
|
|
timestamp_us,
|
|
channel_frames: vec![CanonicalCsiFrame {
|
|
amplitude: amp,
|
|
phase,
|
|
hardware_type: HardwareType::Esp32S3,
|
|
}],
|
|
frequencies_mhz: vec![2412],
|
|
coherence: 0.9,
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn fuse_single_node_fallback() {
|
|
let fuser = MultistaticFuser::new();
|
|
let frames = vec![make_node_frame(0, 1000, 56, 1.0)];
|
|
let fused = fuser.fuse(&frames).unwrap();
|
|
assert_eq!(fused.active_nodes, 1);
|
|
assert_eq!(fused.fused_amplitude.len(), 56);
|
|
assert!((fused.cross_node_coherence - 1.0).abs() < f32::EPSILON);
|
|
}
|
|
|
|
#[test]
|
|
fn fuse_two_identical_nodes() {
|
|
let fuser = MultistaticFuser::new();
|
|
let f0 = make_node_frame(0, 1000, 56, 1.0);
|
|
let f1 = make_node_frame(1, 1001, 56, 1.0);
|
|
let fused = fuser.fuse(&[f0, f1]).unwrap();
|
|
assert_eq!(fused.active_nodes, 2);
|
|
assert_eq!(fused.fused_amplitude.len(), 56);
|
|
// Identical nodes -> high coherence
|
|
assert!(fused.cross_node_coherence > 0.5);
|
|
}
|
|
|
|
#[test]
|
|
fn fuse_four_nodes() {
|
|
let fuser = MultistaticFuser::new();
|
|
let frames: Vec<MultiBandCsiFrame> = (0..4)
|
|
.map(|i| make_node_frame(i, 1000 + i as u64, 56, 1.0 + 0.1 * i as f32))
|
|
.collect();
|
|
let fused = fuser.fuse(&frames).unwrap();
|
|
assert_eq!(fused.active_nodes, 4);
|
|
}
|
|
|
|
// ===== ADR-137 fusion-quality scoring =====
|
|
|
|
#[test]
|
|
fn ac_fuse_scored_tight_alignment_no_contradiction() {
|
|
use super::super::fusion_quality::{EvidenceRef, FamilyId};
|
|
let fuser = MultistaticFuser::new();
|
|
// Two identical nodes, 1 us apart (< soft_guard 1000 us): no contradiction.
|
|
let f0 = make_node_frame(0, 1000, 56, 1.0);
|
|
let f1 = make_node_frame(1, 1001, 56, 1.0);
|
|
let (fused, score) = fuser.fuse_scored(&[f0, f1], 0.85).unwrap();
|
|
|
|
assert_eq!(score.family_id, FamilyId::MultistaticCsi);
|
|
assert_eq!(score.per_node_weights.len(), 2);
|
|
assert!((score.per_node_weights.iter().sum::<f32>() - 1.0).abs() < 1e-4);
|
|
assert_eq!(score.capture_ns, fused.timestamp_us * 1000);
|
|
// Identical nodes → high coherence → gate evidence present.
|
|
assert!(score
|
|
.evidence_refs
|
|
.iter()
|
|
.any(|e| matches!(e, EvidenceRef::CoherenceGateThreshold { .. })));
|
|
assert!(score
|
|
.evidence_refs
|
|
.iter()
|
|
.any(|e| matches!(e, EvidenceRef::WeightEntropy { n_nodes: 2, .. })));
|
|
assert!(!score.forces_privacy_demotion(), "tight alignment ⇒ no demotion");
|
|
}
|
|
|
|
#[test]
|
|
fn ac_fuse_scored_loose_alignment_flags_soft_contradiction() {
|
|
use super::super::fusion_quality::ContradictionFlag;
|
|
// guard 5000 us; spread 2000 us is within guard but > soft_guard 1000 us.
|
|
let fuser = MultistaticFuser::new();
|
|
let f0 = make_node_frame(0, 1000, 56, 1.0);
|
|
let f1 = make_node_frame(1, 3000, 56, 1.0);
|
|
let (_fused, score) = fuser.fuse_scored(&[f0, f1], 0.85).unwrap();
|
|
|
|
assert!(score.forces_privacy_demotion(), "loose alignment ⇒ demotion");
|
|
assert!(matches!(
|
|
score.contradiction_flags[0],
|
|
ContradictionFlag::TimestampMismatch { spread_ns: 2_000_000, soft_guard_ns: 1_000_000 }
|
|
));
|
|
// Penalized coherence is strictly below base when a contradiction fires.
|
|
assert!(score.penalized_coherence() < score.base_coherence);
|
|
}
|
|
|
|
#[test]
|
|
fn ac_fuse_scored_calibrated_agreement_sets_id() {
|
|
use super::super::fusion_quality::{CalibrationId, EvidenceRef};
|
|
let fuser = MultistaticFuser::new();
|
|
let f0 = make_node_frame(0, 1000, 56, 1.0);
|
|
let f1 = make_node_frame(1, 1001, 56, 1.0);
|
|
let cal = CalibrationId(0xCAFE);
|
|
let (_f, score) = fuser
|
|
.fuse_scored_calibrated(&[f0, f1], &[Some(cal), Some(cal)], 0.85)
|
|
.unwrap();
|
|
assert_eq!(score.calibration_id, Some(cal), "agreed calibration recorded");
|
|
assert!(score
|
|
.evidence_refs
|
|
.iter()
|
|
.any(|e| matches!(e, EvidenceRef::CalibrationApplied { calibration_id, .. } if *calibration_id == cal)));
|
|
assert!(!score.forces_privacy_demotion());
|
|
}
|
|
|
|
#[test]
|
|
fn ac_fuse_scored_calibration_mismatch_flags_and_nulls_id() {
|
|
use super::super::fusion_quality::{CalibrationId, ContradictionFlag};
|
|
let fuser = MultistaticFuser::new();
|
|
let f0 = make_node_frame(0, 1000, 56, 1.0);
|
|
let f1 = make_node_frame(1, 1001, 56, 1.0);
|
|
// Two nodes, DIFFERENT calibration epochs → mismatch.
|
|
let (_f, score) = fuser
|
|
.fuse_scored_calibrated(&[f0, f1], &[Some(CalibrationId(1)), Some(CalibrationId(2))], 0.85)
|
|
.unwrap();
|
|
assert_eq!(score.calibration_id, None, "mismatch ⇒ no single calibration id");
|
|
assert!(score
|
|
.contradiction_flags
|
|
.iter()
|
|
.any(|c| matches!(c, ContradictionFlag::CalibrationIdMismatch { disagreeing: 1, .. })));
|
|
assert!(score.forces_privacy_demotion(), "mismatch forces demotion");
|
|
}
|
|
|
|
#[test]
|
|
fn ac_fuse_scored_hard_guard_still_errors() {
|
|
// Beyond the hard guard interval, fuse_scored errors like fuse.
|
|
let config = MultistaticConfig {
|
|
guard_interval_us: 100,
|
|
..Default::default()
|
|
};
|
|
let fuser = MultistaticFuser::with_config(config);
|
|
let f0 = make_node_frame(0, 0, 56, 1.0);
|
|
let f1 = make_node_frame(1, 200, 56, 1.0);
|
|
assert!(matches!(
|
|
fuser.fuse_scored(&[f0, f1], 0.85),
|
|
Err(MultistaticError::TimestampMismatch { .. })
|
|
));
|
|
}
|
|
|
|
#[test]
|
|
fn empty_frames_error() {
|
|
let fuser = MultistaticFuser::new();
|
|
assert!(matches!(fuser.fuse(&[]), Err(MultistaticError::NoFrames)));
|
|
}
|
|
|
|
#[test]
|
|
fn timestamp_mismatch_error() {
|
|
let config = MultistaticConfig {
|
|
guard_interval_us: 100,
|
|
..Default::default()
|
|
};
|
|
let fuser = MultistaticFuser::with_config(config);
|
|
let f0 = make_node_frame(0, 0, 56, 1.0);
|
|
let f1 = make_node_frame(1, 200, 56, 1.0);
|
|
assert!(matches!(
|
|
fuser.fuse(&[f0, f1]),
|
|
Err(MultistaticError::TimestampMismatch { .. })
|
|
));
|
|
}
|
|
|
|
#[test]
|
|
fn dimension_mismatch_error() {
|
|
let fuser = MultistaticFuser::new();
|
|
let f0 = make_node_frame(0, 1000, 56, 1.0);
|
|
let f1 = make_node_frame(1, 1001, 30, 1.0);
|
|
assert!(matches!(
|
|
fuser.fuse(&[f0, f1]),
|
|
Err(MultistaticError::DimensionMismatch { .. })
|
|
));
|
|
}
|
|
|
|
#[test]
|
|
fn node_positions_set_and_retrieved() {
|
|
let mut fuser = MultistaticFuser::new();
|
|
let positions = vec![[0.0, 0.0, 1.0], [3.0, 0.0, 1.0]];
|
|
fuser.set_node_positions(positions.clone());
|
|
assert_eq!(fuser.node_positions(), &positions[..]);
|
|
}
|
|
|
|
#[test]
|
|
fn fused_positions_filled() {
|
|
let mut fuser = MultistaticFuser::new();
|
|
fuser.set_node_positions(vec![[1.0, 2.0, 3.0]]);
|
|
let frames = vec![
|
|
make_node_frame(0, 100, 56, 1.0),
|
|
make_node_frame(1, 101, 56, 1.0),
|
|
];
|
|
let fused = fuser.fuse(&frames).unwrap();
|
|
assert_eq!(fused.node_positions[0], [1.0, 2.0, 3.0]);
|
|
assert_eq!(fused.node_positions[1], [0.0, 0.0, 0.0]); // default
|
|
}
|
|
|
|
#[test]
|
|
fn geometric_diversity_single_node() {
|
|
assert_eq!(geometric_diversity(&[[0.0, 0.0, 0.0]]), 0.0);
|
|
}
|
|
|
|
#[test]
|
|
fn geometric_diversity_two_opposite() {
|
|
let score = geometric_diversity(&[[-1.0, 0.0, 0.0], [1.0, 0.0, 0.0]]);
|
|
assert!(
|
|
score > 0.8,
|
|
"Two opposite nodes should have high diversity: {}",
|
|
score
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn geometric_diversity_four_corners() {
|
|
let score = geometric_diversity(&[
|
|
[0.0, 0.0, 0.0],
|
|
[5.0, 0.0, 0.0],
|
|
[5.0, 5.0, 0.0],
|
|
[0.0, 5.0, 0.0],
|
|
]);
|
|
assert!(
|
|
score > 0.7,
|
|
"Four corners should have good diversity: {}",
|
|
score
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn weight_coherence_uniform() {
|
|
let weights = vec![0.25, 0.25, 0.25, 0.25];
|
|
let c = compute_weight_coherence(&weights);
|
|
assert!((c - 1.0).abs() < 0.01);
|
|
}
|
|
|
|
#[test]
|
|
fn weight_coherence_single_dominant() {
|
|
let weights = vec![0.97, 0.01, 0.01, 0.01];
|
|
let c = compute_weight_coherence(&weights);
|
|
assert!(
|
|
c < 0.3,
|
|
"Single dominant node should have low coherence: {}",
|
|
c
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn default_config() {
|
|
let cfg = MultistaticConfig::default();
|
|
assert_eq!(cfg.guard_interval_us, 5000);
|
|
assert_eq!(cfg.min_nodes, 2);
|
|
assert!((cfg.attention_temperature - 1.0).abs() < f32::EPSILON);
|
|
assert!(cfg.enable_person_separation);
|
|
}
|
|
|
|
#[test]
|
|
fn person_cluster_creation() {
|
|
let cluster = PersonCluster {
|
|
id: 0,
|
|
link_indices: vec![0, 1, 3],
|
|
intra_correlation: 0.85,
|
|
};
|
|
assert_eq!(cluster.link_indices.len(), 3);
|
|
}
|
|
}
|