83 lines
2.8 KiB
Rust
83 lines
2.8 KiB
Rust
//! UWB-based GPS anti-spoofing: cross-validates GPS position against UWB ranging.
|
|
|
|
use crate::types::{NodeId, Position3D};
|
|
|
|
/// Cross-validates GPS against UWB ranging to neighbours.
|
|
pub struct UwbAntiSpoofing {
|
|
/// Tolerance for GPS vs UWB distance discrepancy, metres.
|
|
pub tolerance_m: f64,
|
|
/// Minimum number of UWB neighbours required for a valid cross-check.
|
|
pub min_neighbors: usize,
|
|
}
|
|
|
|
impl UwbAntiSpoofing {
|
|
pub fn new(tolerance_m: f64, min_neighbors: usize) -> Self {
|
|
Self { tolerance_m, min_neighbors }
|
|
}
|
|
|
|
/// Returns `true` if the GPS position is consistent with UWB ranging data.
|
|
pub fn is_gps_valid(
|
|
&self,
|
|
gps_position: &Position3D,
|
|
uwb_ranges: &[(NodeId, f64)],
|
|
neighbor_gps: &[(NodeId, Position3D)],
|
|
) -> bool {
|
|
if uwb_ranges.len() < self.min_neighbors {
|
|
// Not enough UWB anchors to validate — allow through with warning
|
|
return true;
|
|
}
|
|
|
|
let validated_count = uwb_ranges
|
|
.iter()
|
|
.filter_map(|(id, uwb_dist)| {
|
|
neighbor_gps
|
|
.iter()
|
|
.find(|(nid, _)| nid == id)
|
|
.map(|(_, ngps)| {
|
|
let gps_dist = gps_position.distance_to(ngps);
|
|
(gps_dist - uwb_dist).abs() <= self.tolerance_m
|
|
})
|
|
})
|
|
.filter(|&ok| ok)
|
|
.count();
|
|
|
|
// Require majority of ranges to be consistent
|
|
validated_count * 2 >= uwb_ranges.len()
|
|
}
|
|
}
|
|
|
|
impl Default for UwbAntiSpoofing {
|
|
fn default() -> Self {
|
|
Self::new(2.0, 2)
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_consistent_gps_valid() {
|
|
let anti = UwbAntiSpoofing::new(2.0, 2);
|
|
let gps = Position3D { x: 0.0, y: 0.0, z: 0.0 };
|
|
let n1_pos = Position3D { x: 10.0, y: 0.0, z: 0.0 };
|
|
let n2_pos = Position3D { x: 0.0, y: 10.0, z: 0.0 };
|
|
let uwb_ranges = vec![(NodeId(1), 10.0), (NodeId(2), 10.0)];
|
|
let neighbor_gps = vec![(NodeId(1), n1_pos), (NodeId(2), n2_pos)];
|
|
assert!(anti.is_gps_valid(&gps, &uwb_ranges, &neighbor_gps));
|
|
}
|
|
|
|
#[test]
|
|
fn test_spoofed_gps_invalid() {
|
|
let anti = UwbAntiSpoofing::new(2.0, 2);
|
|
// GPS claims (0,0) but UWB says drone is 50m from both neighbours
|
|
let gps = Position3D { x: 0.0, y: 0.0, z: 0.0 };
|
|
let n1_pos = Position3D { x: 10.0, y: 0.0, z: 0.0 };
|
|
let n2_pos = Position3D { x: 0.0, y: 10.0, z: 0.0 };
|
|
// UWB reports 50m but GPS only shows 10m — spoof detected
|
|
let uwb_ranges = vec![(NodeId(1), 50.0), (NodeId(2), 50.0)];
|
|
let neighbor_gps = vec![(NodeId(1), n1_pos), (NodeId(2), n2_pos)];
|
|
assert!(!anti.is_gps_valid(&gps, &uwb_ranges, &neighbor_gps));
|
|
}
|
|
}
|