181 lines
6.5 KiB
Rust
181 lines
6.5 KiB
Rust
use crate::types::{NodeId, Position3D, CsiDetection};
|
|
|
|
/// A fused detection result from multiple drone viewpoints.
|
|
#[derive(Debug, Clone)]
|
|
pub struct FusedDetection {
|
|
pub confidence: f32,
|
|
pub estimated_position: Position3D,
|
|
pub contributing_drones: Vec<NodeId>,
|
|
/// Localization uncertainty ellipse (std dev in metres).
|
|
pub uncertainty_m: f64,
|
|
}
|
|
|
|
/// Geometric diversity metric (Cramer-Rao bound proxy).
|
|
/// More diverse viewpoints -> lower bound -> better localization.
|
|
fn geometric_diversity_index(positions: &[Position3D]) -> f64 {
|
|
if positions.len() < 2 {
|
|
return 0.0;
|
|
}
|
|
// Compute average pairwise angular separation
|
|
let n = positions.len();
|
|
let centroid = Position3D {
|
|
x: positions.iter().map(|p| p.x).sum::<f64>() / n as f64,
|
|
y: positions.iter().map(|p| p.y).sum::<f64>() / n as f64,
|
|
z: positions.iter().map(|p| p.z).sum::<f64>() / n as f64,
|
|
};
|
|
|
|
let mut total_angle = 0.0_f64;
|
|
let mut pairs = 0;
|
|
for i in 0..n {
|
|
for j in (i + 1)..n {
|
|
let a = (positions[i].x - centroid.x, positions[i].y - centroid.y);
|
|
let b = (positions[j].x - centroid.x, positions[j].y - centroid.y);
|
|
let dot = a.0 * b.0 + a.1 * b.1;
|
|
let mag_a = (a.0 * a.0 + a.1 * a.1).sqrt().max(1e-9);
|
|
let mag_b = (b.0 * b.0 + b.1 * b.1).sqrt().max(1e-9);
|
|
let cos_angle = (dot / (mag_a * mag_b)).clamp(-1.0, 1.0);
|
|
total_angle += cos_angle.acos();
|
|
pairs += 1;
|
|
}
|
|
}
|
|
|
|
if pairs > 0 { total_angle / pairs as f64 } else { 0.0 }
|
|
}
|
|
|
|
/// Multi-drone CSI fusion via confidence-weighted position averaging with geometric bias.
|
|
pub struct MultiViewFusion {
|
|
/// Minimum number of independent viewpoints required to produce a fused result.
|
|
pub min_viewpoints: usize,
|
|
/// Minimum confidence of individual detections to include in fusion.
|
|
pub min_confidence: f32,
|
|
}
|
|
|
|
impl Default for MultiViewFusion {
|
|
fn default() -> Self {
|
|
Self { min_viewpoints: 2, min_confidence: 0.5 }
|
|
}
|
|
}
|
|
|
|
impl MultiViewFusion {
|
|
/// Fuse multiple CSI detections from different drone viewpoints.
|
|
/// Returns None if fewer than min_viewpoints pass the confidence threshold.
|
|
pub fn fuse(
|
|
&self,
|
|
detections: &[CsiDetection],
|
|
drone_positions: &[(NodeId, Position3D)],
|
|
) -> Option<FusedDetection> {
|
|
// Filter by confidence and require estimated position
|
|
let valid: Vec<(&CsiDetection, &Position3D)> = detections
|
|
.iter()
|
|
.filter(|d| d.confidence >= self.min_confidence && d.victim_position.is_some())
|
|
.filter_map(|d| {
|
|
let drone_pos = drone_positions
|
|
.iter()
|
|
.find(|(id, _)| *id == d.drone_id)
|
|
.map(|(_, p)| p)?;
|
|
Some((d, drone_pos))
|
|
})
|
|
.collect();
|
|
|
|
if valid.len() < self.min_viewpoints {
|
|
return None;
|
|
}
|
|
|
|
// Compute geometric diversity index for uncertainty estimate
|
|
let drone_pos_list: Vec<Position3D> = valid.iter().map(|(_, p)| **p).collect();
|
|
let gdi = geometric_diversity_index(&drone_pos_list);
|
|
|
|
// Weighted average of victim position estimates
|
|
let total_weight: f32 = valid.iter().map(|(d, _)| d.confidence).sum();
|
|
let mut fused_x = 0.0_f64;
|
|
let mut fused_y = 0.0_f64;
|
|
let mut fused_z = 0.0_f64;
|
|
let mut fused_conf = 0.0_f32;
|
|
|
|
for (det, _) in &valid {
|
|
let w = det.confidence / total_weight;
|
|
let vp = det.victim_position.unwrap();
|
|
fused_x += w as f64 * vp.x;
|
|
fused_y += w as f64 * vp.y;
|
|
fused_z += w as f64 * vp.z;
|
|
fused_conf += w * det.confidence;
|
|
}
|
|
|
|
// Uncertainty shrinks with geometric diversity and number of viewpoints:
|
|
// baseline 5 m (single drone) -> scales down by sqrt(n) and gdi factor
|
|
let base_uncertainty_m = 5.0;
|
|
let n = valid.len() as f64;
|
|
let gdi_factor = (1.0 + gdi / std::f64::consts::PI).clamp(1.0, 2.0);
|
|
let uncertainty_m = base_uncertainty_m / (n.sqrt() * gdi_factor);
|
|
|
|
Some(FusedDetection {
|
|
confidence: fused_conf,
|
|
estimated_position: Position3D { x: fused_x, y: fused_y, z: fused_z },
|
|
contributing_drones: valid.iter().map(|(d, _)| d.drone_id).collect(),
|
|
uncertainty_m,
|
|
})
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_fusion_single_view_insufficient() {
|
|
let fusion = MultiViewFusion { min_viewpoints: 2, min_confidence: 0.5 };
|
|
let det = CsiDetection {
|
|
drone_id: NodeId(0),
|
|
confidence: 0.9,
|
|
victim_position: Some(Position3D { x: 10.0, y: 5.0, z: 0.0 }),
|
|
timestamp_ms: 0,
|
|
};
|
|
let result = fusion.fuse(&[det], &[(NodeId(0), Position3D::zero())]);
|
|
assert!(result.is_none(), "single viewpoint should not produce fusion");
|
|
}
|
|
|
|
#[test]
|
|
fn test_fusion_three_views() {
|
|
let fusion = MultiViewFusion::default();
|
|
let victim = Position3D { x: 50.0, y: 50.0, z: 0.0 };
|
|
let detections = vec![
|
|
CsiDetection {
|
|
drone_id: NodeId(0),
|
|
confidence: 0.85,
|
|
victim_position: Some(Position3D { x: 51.0, y: 49.0, z: 0.0 }),
|
|
timestamp_ms: 0,
|
|
},
|
|
CsiDetection {
|
|
drone_id: NodeId(1),
|
|
confidence: 0.78,
|
|
victim_position: Some(Position3D { x: 49.0, y: 51.0, z: 0.0 }),
|
|
timestamp_ms: 0,
|
|
},
|
|
CsiDetection {
|
|
drone_id: NodeId(2),
|
|
confidence: 0.92,
|
|
victim_position: Some(Position3D { x: 50.0, y: 50.0, z: 0.0 }),
|
|
timestamp_ms: 0,
|
|
},
|
|
];
|
|
let positions = vec![
|
|
(NodeId(0), Position3D { x: 0.0, y: 0.0, z: -30.0 }),
|
|
(NodeId(1), Position3D { x: 100.0, y: 0.0, z: -30.0 }),
|
|
(NodeId(2), Position3D { x: 50.0, y: 86.6, z: -30.0 }), // equilateral triangle
|
|
];
|
|
|
|
let result = fusion.fuse(&detections, &positions).unwrap();
|
|
let err = result.estimated_position.distance_to(&victim);
|
|
assert!(
|
|
err < 3.0,
|
|
"fusion error {} m should be < 3 m for 3 equilateral viewpoints",
|
|
err
|
|
);
|
|
assert!(
|
|
result.uncertainty_m < 5.0,
|
|
"uncertainty {} should be < 5 m single-drone baseline",
|
|
result.uncertainty_m
|
|
);
|
|
}
|
|
}
|