5541926e6a
- tomography.rs: use checked_mul for nx*ny*nz to prevent integer overflow on adversarial grid configurations - phase_align.rs: add defensive bounds check in mean_phase_on_indices to prevent panic on out-of-range subcarrier indices - multistatic.rs: stabilize softmax in attention_weighted_fusion with max-subtraction to prevent exp() overflow on extreme similarity values Co-Authored-By: claude-flow <ruv@ruv.net>
563 lines
18 KiB
Rust
563 lines
18 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.
|
|
//!
|
|
//! # RuVector Integration
|
|
//!
|
|
//! - `ruvector-attn-mincut` for cross-node spectrogram attention gating
|
|
//! - `ruvector-mincut` for person separation (DynamicMinCut)
|
|
|
|
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,
|
|
/// 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,
|
|
}
|
|
|
|
impl Default for MultistaticConfig {
|
|
fn default() -> Self {
|
|
Self {
|
|
guard_interval_us: 5000,
|
|
min_nodes: 2,
|
|
attention_temperature: 1.0,
|
|
enable_person_separation: true,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Multistatic frame fuser.
|
|
///
|
|
/// Collects per-node multi-band frames and produces a single fused
|
|
/// sensing frame per TDMA cycle.
|
|
#[derive(Debug)]
|
|
pub struct MultistaticFuser {
|
|
config: MultistaticConfig,
|
|
/// Node positions in 3D space (meters).
|
|
node_positions: Vec<[f32; 3]>,
|
|
}
|
|
|
|
impl MultistaticFuser {
|
|
/// Create a fuser with default configuration and no node positions.
|
|
pub fn new() -> Self {
|
|
Self {
|
|
config: MultistaticConfig::default(),
|
|
node_positions: Vec::new(),
|
|
}
|
|
}
|
|
|
|
/// Create a fuser with custom configuration.
|
|
pub fn with_config(config: MultistaticConfig) -> Self {
|
|
Self {
|
|
config,
|
|
node_positions: Vec::new(),
|
|
}
|
|
}
|
|
|
|
/// 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, 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)
|
|
};
|
|
|
|
// 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,
|
|
})
|
|
}
|
|
}
|
|
|
|
impl Default for MultistaticFuser {
|
|
fn default() -> Self {
|
|
Self::new()
|
|
}
|
|
}
|
|
|
|
/// 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_nodes = amplitudes.len();
|
|
let n_sub = amplitudes[0].len();
|
|
|
|
// Compute 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;
|
|
}
|
|
|
|
// Compute attention weights based on similarity to consensus
|
|
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 {
|
|
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);
|
|
let similarity = dot / denom;
|
|
logits[n] = similarity / temperature;
|
|
}
|
|
|
|
// Numerically stable softmax: subtract max to prevent exp() overflow
|
|
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;
|
|
}
|
|
|
|
// 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 coherence from attention weights.
|
|
///
|
|
/// Returns 1.0 when all weights are equal (all nodes agree),
|
|
/// and approaches 0.0 when a single node dominates.
|
|
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;
|
|
let diversity = (ideal_gap / max_gap.max(1e-6)).clamp(0.0, 1.0);
|
|
diversity
|
|
}
|
|
|
|
/// 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);
|
|
}
|
|
|
|
#[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);
|
|
}
|
|
}
|