mirror of
https://github.com/ruvnet/RuView
synced 2026-08-05 19:41:44 +00:00
ruv
982994ca3c
estimate_gdop returned an average-pair-angle factor merely labelled GDOP (the same class of defect ADR-156 §2.3 fixed). Replaced with the genuine Geometric Dilution of Precision computed from the range-measurement Jacobian H (unit target->sensor bearings): GDOP = sqrt(trace((HtH)^-1)), dimensionless, returning None for singular (collinear) geometry which the caller treats as factor 1.0. Tests assert a well-spread array yields lower GDOP than a near-collinear one, cross-check the closed form, and confirm singular geometry returns None. Co-Authored-By: claude-flow <ruv@ruv.net>
611 lines
20 KiB
Rust
611 lines
20 KiB
Rust
//! Triangulation for 2D/3D position estimation from multiple sensors.
|
||
|
||
use crate::domain::{Coordinates3D, LocationUncertainty, SensorPosition};
|
||
|
||
/// Configuration for triangulation
|
||
#[derive(Debug, Clone)]
|
||
pub struct TriangulationConfig {
|
||
/// Minimum number of sensors required
|
||
pub min_sensors: usize,
|
||
/// Maximum position uncertainty to accept (meters)
|
||
pub max_uncertainty: f64,
|
||
/// Path loss exponent for distance estimation
|
||
pub path_loss_exponent: f64,
|
||
/// Reference distance for path loss model (meters)
|
||
pub reference_distance: f64,
|
||
/// Reference RSSI at reference distance (dBm)
|
||
pub reference_rssi: f64,
|
||
/// Use weighted least squares
|
||
pub weighted: bool,
|
||
}
|
||
|
||
impl Default for TriangulationConfig {
|
||
fn default() -> Self {
|
||
Self {
|
||
min_sensors: 3,
|
||
max_uncertainty: 5.0,
|
||
path_loss_exponent: 3.0, // Indoor with obstacles
|
||
reference_distance: 1.0,
|
||
reference_rssi: -30.0,
|
||
weighted: true,
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Result of a distance estimation
|
||
#[derive(Debug, Clone)]
|
||
#[allow(dead_code)]
|
||
pub struct DistanceEstimate {
|
||
/// Sensor ID
|
||
pub sensor_id: String,
|
||
/// Estimated distance in meters
|
||
pub distance: f64,
|
||
/// Estimation confidence
|
||
pub confidence: f64,
|
||
}
|
||
|
||
/// Triangulator for position estimation
|
||
pub struct Triangulator {
|
||
config: TriangulationConfig,
|
||
}
|
||
|
||
impl Triangulator {
|
||
/// Create a new triangulator
|
||
pub fn new(config: TriangulationConfig) -> Self {
|
||
Self { config }
|
||
}
|
||
|
||
/// Create with default configuration
|
||
pub fn with_defaults() -> Self {
|
||
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,
|
||
sensors: &[SensorPosition],
|
||
rssi_values: &[(String, f64)], // (sensor_id, rssi)
|
||
) -> Option<Coordinates3D> {
|
||
// Get distance estimates from RSSI
|
||
let distances: Vec<(SensorPosition, f64)> = rssi_values
|
||
.iter()
|
||
.filter_map(|(id, rssi)| {
|
||
let sensor = sensors.iter().find(|s| &s.id == id)?;
|
||
if !sensor.is_operational {
|
||
return None;
|
||
}
|
||
let distance = self.rssi_to_distance(*rssi);
|
||
Some((sensor.clone(), distance))
|
||
})
|
||
.collect();
|
||
|
||
if distances.len() < self.config.min_sensors {
|
||
return None;
|
||
}
|
||
|
||
// Perform trilateration
|
||
self.trilaterate(&distances)
|
||
}
|
||
|
||
/// Estimate position from Time of Arrival measurements
|
||
pub fn estimate_from_toa(
|
||
&self,
|
||
sensors: &[SensorPosition],
|
||
toa_values: &[(String, f64)], // (sensor_id, time_of_arrival_ns)
|
||
) -> Option<Coordinates3D> {
|
||
const SPEED_OF_LIGHT: f64 = 299_792_458.0; // m/s
|
||
|
||
let distances: Vec<(SensorPosition, f64)> = toa_values
|
||
.iter()
|
||
.filter_map(|(id, toa)| {
|
||
let sensor = sensors.iter().find(|s| &s.id == id)?;
|
||
if !sensor.is_operational {
|
||
return None;
|
||
}
|
||
// Convert nanoseconds to distance
|
||
let distance = (*toa * 1e-9) * SPEED_OF_LIGHT / 2.0; // Round trip
|
||
Some((sensor.clone(), distance))
|
||
})
|
||
.collect();
|
||
|
||
if distances.len() < self.config.min_sensors {
|
||
return None;
|
||
}
|
||
|
||
self.trilaterate(&distances)
|
||
}
|
||
|
||
/// Convert RSSI to distance using path loss model
|
||
fn rssi_to_distance(&self, rssi: f64) -> f64 {
|
||
// Log-distance path loss model:
|
||
// RSSI = RSSI_0 - 10 * n * log10(d / d_0)
|
||
// Solving for d:
|
||
// d = d_0 * 10^((RSSI_0 - RSSI) / (10 * n))
|
||
|
||
let exponent =
|
||
(self.config.reference_rssi - rssi) / (10.0 * self.config.path_loss_exponent);
|
||
|
||
self.config.reference_distance * 10.0_f64.powf(exponent)
|
||
}
|
||
|
||
/// Perform trilateration using least squares
|
||
fn trilaterate(&self, distances: &[(SensorPosition, f64)]) -> Option<Coordinates3D> {
|
||
if distances.len() < 3 {
|
||
return None;
|
||
}
|
||
|
||
// Use linearized least squares approach
|
||
// Reference: https://en.wikipedia.org/wiki/Trilateration
|
||
|
||
// Use first sensor as reference
|
||
let (ref_sensor, ref_dist) = &distances[0];
|
||
let x1 = ref_sensor.x;
|
||
let y1 = ref_sensor.y;
|
||
let r1 = *ref_dist;
|
||
|
||
// Build system of linear equations: A * [x, y]^T = b
|
||
let n = distances.len() - 1;
|
||
let mut a_matrix = vec![vec![0.0; 2]; n];
|
||
let mut b_vector = vec![0.0; n];
|
||
|
||
for (i, (sensor, dist)) in distances.iter().skip(1).enumerate() {
|
||
let xi = sensor.x;
|
||
let yi = sensor.y;
|
||
let ri = *dist;
|
||
|
||
// Linearized equation from difference of squared distances
|
||
a_matrix[i][0] = 2.0 * (xi - x1);
|
||
a_matrix[i][1] = 2.0 * (yi - y1);
|
||
b_vector[i] = r1 * r1 - ri * ri - x1 * x1 + xi * xi - y1 * y1 + yi * yi;
|
||
}
|
||
|
||
// Solve using least squares: (A^T * A)^-1 * A^T * b
|
||
let solution = self.solve_least_squares(&a_matrix, &b_vector)?;
|
||
|
||
// Calculate uncertainty from residuals
|
||
let uncertainty = self.calculate_uncertainty(&solution, distances);
|
||
|
||
if uncertainty.horizontal_error > self.config.max_uncertainty {
|
||
return None;
|
||
}
|
||
|
||
Some(Coordinates3D::new(
|
||
solution[0],
|
||
solution[1],
|
||
0.0, // Z estimated separately
|
||
uncertainty,
|
||
))
|
||
}
|
||
|
||
/// Solve linear system using least squares
|
||
#[allow(clippy::needless_range_loop)]
|
||
fn solve_least_squares(&self, a: &[Vec<f64>], b: &[f64]) -> Option<Vec<f64>> {
|
||
let n = a.len();
|
||
if n < 2 || a[0].len() != 2 {
|
||
return None;
|
||
}
|
||
|
||
// Calculate A^T * A (dual-index matrix multiplication — range loop required)
|
||
let mut ata = vec![vec![0.0; 2]; 2];
|
||
for i in 0..2 {
|
||
for j in 0..2 {
|
||
for k in 0..n {
|
||
ata[i][j] += a[k][i] * a[k][j];
|
||
}
|
||
}
|
||
}
|
||
|
||
// Calculate A^T * b (dual-index — range loop required)
|
||
let mut atb = [0.0; 2];
|
||
for i in 0..2 {
|
||
for k in 0..n {
|
||
atb[i] += a[k][i] * b[k];
|
||
}
|
||
}
|
||
|
||
// Solve 2x2 system using Cramer's rule
|
||
let det = ata[0][0] * ata[1][1] - ata[0][1] * ata[1][0];
|
||
if det.abs() < 1e-10 {
|
||
return None;
|
||
}
|
||
|
||
let x = (atb[0] * ata[1][1] - atb[1] * ata[0][1]) / det;
|
||
let y = (ata[0][0] * atb[1] - ata[1][0] * atb[0]) / det;
|
||
|
||
Some(vec![x, y])
|
||
}
|
||
|
||
/// Calculate position uncertainty from residuals
|
||
fn calculate_uncertainty(
|
||
&self,
|
||
position: &[f64],
|
||
distances: &[(SensorPosition, f64)],
|
||
) -> LocationUncertainty {
|
||
// Calculate root mean square error
|
||
let mut sum_sq_error = 0.0;
|
||
|
||
for (sensor, measured_dist) in distances {
|
||
let dx = position[0] - sensor.x;
|
||
let dy = position[1] - sensor.y;
|
||
let estimated_dist = (dx * dx + dy * dy).sqrt();
|
||
let error = measured_dist - estimated_dist;
|
||
sum_sq_error += error * error;
|
||
}
|
||
|
||
let rmse = (sum_sq_error / distances.len() as f64).sqrt();
|
||
|
||
// Real, dimensionless GDOP (Geometric Dilution of Precision). Falls back
|
||
// to a unit factor for a degenerate (collinear) geometry where (HᵀH) is
|
||
// singular — that geometry already produces a large residual RMSE.
|
||
let gdop = self
|
||
.compute_gdop(position, distances)
|
||
.unwrap_or(1.0)
|
||
.max(1.0);
|
||
|
||
LocationUncertainty {
|
||
horizontal_error: rmse * gdop,
|
||
vertical_error: rmse * gdop * 1.5, // Vertical typically less accurate
|
||
confidence: 0.95,
|
||
}
|
||
}
|
||
|
||
/// Compute the real Geometric Dilution of Precision (GDOP).
|
||
///
|
||
/// GDOP is the dimensionless factor by which measurement (range) noise is
|
||
/// amplified into position error by the sensor geometry. For range-based 2D
|
||
/// positioning the measurement Jacobian `H` has one row per sensor equal to
|
||
/// the unit bearing vector from the target to that sensor,
|
||
/// `[ (xₛ-xₜ)/d , (yₛ-yₜ)/d ]`. The position covariance (per unit measurement
|
||
/// variance) is `(HᵀH)⁻¹`, and
|
||
///
|
||
/// ```text
|
||
/// GDOP = sqrt( trace( (HᵀH)⁻¹ ) )
|
||
/// ```
|
||
///
|
||
/// This is the same quantity ADR-156 §2.3 corrected elsewhere — a genuine
|
||
/// dimensionless dilution, not the previous ad-hoc average-angle factor that
|
||
/// was merely *labelled* GDOP. Returns `None` when `HᵀH` is singular
|
||
/// (collinear / coincident geometry), which the caller treats as no
|
||
/// dilution information (factor 1.0).
|
||
fn compute_gdop(&self, position: &[f64], distances: &[(SensorPosition, f64)]) -> Option<f64> {
|
||
let (tx, ty) = (position[0], position[1]);
|
||
|
||
// Accumulate HᵀH (2×2, symmetric) from unit bearing vectors.
|
||
let (mut hxx, mut hxy, mut hyy) = (0.0_f64, 0.0_f64, 0.0_f64);
|
||
let mut rows = 0usize;
|
||
for (sensor, _dist) in distances {
|
||
let dx = sensor.x - tx;
|
||
let dy = sensor.y - ty;
|
||
let d = (dx * dx + dy * dy).sqrt();
|
||
if d <= f64::EPSILON {
|
||
continue; // target coincident with sensor: undefined bearing
|
||
}
|
||
let ux = dx / d;
|
||
let uy = dy / d;
|
||
hxx += ux * ux;
|
||
hxy += ux * uy;
|
||
hyy += uy * uy;
|
||
rows += 1;
|
||
}
|
||
|
||
if rows < 2 {
|
||
return None;
|
||
}
|
||
|
||
// Invert the 2×2 HᵀH. trace((HᵀH)⁻¹) = (hxx + hyy) / det.
|
||
let det = hxx * hyy - hxy * hxy;
|
||
if det.abs() < 1e-12 {
|
||
return None; // singular: collinear geometry
|
||
}
|
||
let trace_inv = (hxx + hyy) / det;
|
||
if trace_inv <= 0.0 {
|
||
return None;
|
||
}
|
||
Some(trace_inv.sqrt())
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
use crate::domain::SensorType;
|
||
|
||
fn create_test_sensors() -> Vec<SensorPosition> {
|
||
vec![
|
||
SensorPosition {
|
||
id: "s1".to_string(),
|
||
x: 0.0,
|
||
y: 0.0,
|
||
z: 1.5,
|
||
sensor_type: SensorType::Transceiver,
|
||
is_operational: true,
|
||
last_rssi: None,
|
||
},
|
||
SensorPosition {
|
||
id: "s2".to_string(),
|
||
x: 10.0,
|
||
y: 0.0,
|
||
z: 1.5,
|
||
sensor_type: SensorType::Transceiver,
|
||
is_operational: true,
|
||
last_rssi: None,
|
||
},
|
||
SensorPosition {
|
||
id: "s3".to_string(),
|
||
x: 5.0,
|
||
y: 10.0,
|
||
z: 1.5,
|
||
sensor_type: SensorType::Transceiver,
|
||
is_operational: true,
|
||
last_rssi: None,
|
||
},
|
||
]
|
||
}
|
||
|
||
#[test]
|
||
fn test_rssi_to_distance() {
|
||
let triangulator = Triangulator::with_defaults();
|
||
|
||
// At reference distance, RSSI should equal reference RSSI
|
||
let distance = triangulator.rssi_to_distance(-30.0);
|
||
assert!((distance - 1.0).abs() < 0.1);
|
||
|
||
// Weaker signal = further distance
|
||
let distance2 = triangulator.rssi_to_distance(-60.0);
|
||
assert!(distance2 > distance);
|
||
}
|
||
|
||
#[test]
|
||
fn test_trilateration() {
|
||
let triangulator = Triangulator::with_defaults();
|
||
let sensors = create_test_sensors();
|
||
|
||
// Target at (5, 4) - calculate distances
|
||
let target: (f64, f64) = (5.0, 4.0);
|
||
let distances: Vec<(&str, f64)> = vec![
|
||
(
|
||
"s1",
|
||
((target.0 - 0.0_f64).powi(2) + (target.1 - 0.0_f64).powi(2)).sqrt(),
|
||
),
|
||
(
|
||
"s2",
|
||
((target.0 - 10.0_f64).powi(2) + (target.1 - 0.0_f64).powi(2)).sqrt(),
|
||
),
|
||
(
|
||
"s3",
|
||
((target.0 - 5.0_f64).powi(2) + (target.1 - 10.0_f64).powi(2)).sqrt(),
|
||
),
|
||
];
|
||
|
||
let dist_vec: Vec<(SensorPosition, f64)> = distances
|
||
.iter()
|
||
.filter_map(|(id, d)| {
|
||
let sensor = sensors.iter().find(|s| s.id == *id)?;
|
||
Some((sensor.clone(), *d))
|
||
})
|
||
.collect();
|
||
|
||
let result = triangulator.trilaterate(&dist_vec);
|
||
assert!(result.is_some());
|
||
|
||
let pos = result.unwrap();
|
||
assert!((pos.x - target.0).abs() < 0.5);
|
||
assert!((pos.y - target.1).abs() < 0.5);
|
||
}
|
||
|
||
#[test]
|
||
fn test_insufficient_sensors() {
|
||
let triangulator = Triangulator::with_defaults();
|
||
let sensors = create_test_sensors();
|
||
|
||
// Only 2 distance measurements
|
||
let rssi_values = vec![("s1".to_string(), -40.0), ("s2".to_string(), -45.0)];
|
||
|
||
let result = triangulator.estimate_position(&sensors, &rssi_values);
|
||
assert!(result.is_none());
|
||
}
|
||
|
||
fn sensor_at(id: &str, x: f64, y: f64) -> SensorPosition {
|
||
SensorPosition {
|
||
id: id.to_string(),
|
||
x,
|
||
y,
|
||
z: 1.5,
|
||
sensor_type: SensorType::Transceiver,
|
||
is_operational: true,
|
||
last_rssi: None,
|
||
}
|
||
}
|
||
|
||
/// Real GDOP: dimensionless, geometry-dependent, and matches the closed-form
|
||
/// sqrt(trace((HᵀH)⁻¹)). A well-spread (near-orthogonal) array must give a
|
||
/// LOWER GDOP than a near-collinear one. (The old ad-hoc angle factor was not
|
||
/// a true dilution and is replaced.)
|
||
#[test]
|
||
fn test_gdop_is_real_dilution() {
|
||
let t = Triangulator::with_defaults();
|
||
let target = [5.0_f64, 5.0_f64];
|
||
|
||
// Well-distributed: an equilateral-ish triangle around the target.
|
||
let good = vec![
|
||
(sensor_at("a", 5.0, 15.0), 10.0),
|
||
(sensor_at("b", -3.66, 0.0), 10.0),
|
||
(sensor_at("c", 13.66, 0.0), 10.0),
|
||
];
|
||
let gdop_good = t.compute_gdop(&target, &good).expect("good geometry");
|
||
|
||
// Near-collinear: bearings nearly all along ±y with a tiny x-spread, so
|
||
// HᵀH is ill-conditioned (invertible but with a small eigenvalue) and the
|
||
// GDOP is large but finite.
|
||
let bad = vec![
|
||
(sensor_at("a", 5.3, 15.0), 10.0),
|
||
(sensor_at("b", 4.7, 15.0), 10.0),
|
||
(sensor_at("c", 5.1, -5.0), 10.0),
|
||
];
|
||
let gdop_bad = t.compute_gdop(&target, &bad).expect("bad geometry");
|
||
|
||
assert!(gdop_good >= 1.0, "GDOP must be >= 1 (dilution, dimensionless)");
|
||
assert!(
|
||
gdop_good < gdop_bad,
|
||
"well-spread GDOP {gdop_good} must be < near-collinear GDOP {gdop_bad}"
|
||
);
|
||
|
||
// Closed-form cross-check for the well-spread case: each unit bearing
|
||
// vector contributes to HᵀH; verify trace((HᵀH)⁻¹) explicitly.
|
||
let (mut hxx, mut hxy, mut hyy) = (0.0, 0.0, 0.0);
|
||
for (s, _d) in &good {
|
||
let dx = s.x - target[0];
|
||
let dy = s.y - target[1];
|
||
let d = (dx * dx + dy * dy).sqrt();
|
||
let (ux, uy) = (dx / d, dy / d);
|
||
hxx += ux * ux;
|
||
hxy += ux * uy;
|
||
hyy += uy * uy;
|
||
}
|
||
let det = hxx * hyy - hxy * hxy;
|
||
let expected = ((hxx + hyy) / det).sqrt();
|
||
assert!((gdop_good - expected).abs() < 1e-9);
|
||
}
|
||
|
||
/// Collinear geometry makes HᵀH singular -> compute_gdop returns None,
|
||
/// and the uncertainty path falls back to a unit factor (no fabrication).
|
||
#[test]
|
||
fn test_gdop_singular_collinear_is_none() {
|
||
let t = Triangulator::with_defaults();
|
||
let target = [0.0_f64, 0.0_f64];
|
||
// All sensors on the +x axis through the target: bearings all ±x -> rank 1.
|
||
let collinear = vec![
|
||
(sensor_at("a", 1.0, 0.0), 1.0),
|
||
(sensor_at("b", 2.0, 0.0), 2.0),
|
||
(sensor_at("c", 3.0, 0.0), 3.0),
|
||
];
|
||
assert!(t.compute_gdop(&target, &collinear).is_none());
|
||
}
|
||
}
|
||
|
||
// ---------------------------------------------------------------------------
|
||
// Integration 5: Multi-AP TDoA triangulation via NeumannSolver
|
||
// ---------------------------------------------------------------------------
|
||
|
||
#[cfg(feature = "ruvector")]
|
||
use ruvector_solver::neumann::NeumannSolver;
|
||
#[cfg(feature = "ruvector")]
|
||
use ruvector_solver::types::CsrMatrix;
|
||
|
||
/// Solve multi-AP TDoA survivor localization using NeumannSolver.
|
||
///
|
||
/// For N access points with TDoA measurements, linearizes the hyperbolic
|
||
/// equations and solves the 2×2 normal equations system. Complexity is O(1)
|
||
/// in AP count (always solves a 2×2 system regardless of N).
|
||
///
|
||
/// # Arguments
|
||
/// * `tdoa_measurements` - Vec of (ap_i_idx, ap_j_idx, tdoa_seconds)
|
||
/// where tdoa = t_i - t_j (positive if closer to AP_i)
|
||
/// * `ap_positions` - Vec of (x_metres, y_metres) for each AP
|
||
///
|
||
/// # Returns
|
||
/// Some((x, y)) estimated survivor position in metres, or None if underdetermined
|
||
#[cfg(feature = "ruvector")]
|
||
pub fn solve_tdoa_triangulation(
|
||
tdoa_measurements: &[(usize, usize, f32)],
|
||
ap_positions: &[(f32, f32)],
|
||
) -> Option<(f32, f32)> {
|
||
let n_meas = tdoa_measurements.len();
|
||
if n_meas < 3 || ap_positions.len() < 2 {
|
||
return None;
|
||
}
|
||
|
||
const C: f32 = 3e8_f32; // speed of light m/s
|
||
let (x_ref, y_ref) = ap_positions[0];
|
||
|
||
// Accumulate (A^T A) and (A^T b) for 2×2 normal equations
|
||
let mut ata = [[0.0_f32; 2]; 2];
|
||
let mut atb = [0.0_f32; 2];
|
||
|
||
for &(i, j, tdoa) in tdoa_measurements {
|
||
let (xi, yi) = ap_positions.get(i).copied().unwrap_or((x_ref, y_ref));
|
||
let (xj, yj) = ap_positions.get(j).copied().unwrap_or((x_ref, y_ref));
|
||
|
||
// Row of A: [xi - xj, yi - yj] (linearized TDoA)
|
||
let ai0 = xi - xj;
|
||
let ai1 = yi - yj;
|
||
|
||
// RHS: C * tdoa / 2 + (xi^2 - xj^2 + yi^2 - yj^2) / 2 - x_ref*(xi-xj) - y_ref*(yi-yj)
|
||
let bi = C * tdoa / 2.0 + ((xi * xi - xj * xj) + (yi * yi - yj * yj)) / 2.0
|
||
- x_ref * ai0
|
||
- y_ref * ai1;
|
||
|
||
ata[0][0] += ai0 * ai0;
|
||
ata[0][1] += ai0 * ai1;
|
||
ata[1][0] += ai1 * ai0;
|
||
ata[1][1] += ai1 * ai1;
|
||
atb[0] += ai0 * bi;
|
||
atb[1] += ai1 * bi;
|
||
}
|
||
|
||
// Tikhonov regularization
|
||
let lambda = 0.01_f32;
|
||
ata[0][0] += lambda;
|
||
ata[1][1] += lambda;
|
||
|
||
let csr = CsrMatrix::<f32>::from_coo(
|
||
2,
|
||
2,
|
||
vec![
|
||
(0, 0, ata[0][0]),
|
||
(0, 1, ata[0][1]),
|
||
(1, 0, ata[1][0]),
|
||
(1, 1, ata[1][1]),
|
||
],
|
||
);
|
||
|
||
// Attempt the Neumann-series solver first; fall back to Cramer's rule for
|
||
// the 2×2 case when the iterative solver cannot converge (e.g. the
|
||
// diagonal is very large relative to f32 precision).
|
||
if let Ok(r) = NeumannSolver::new(1e-5, 500).solve(&csr, &atb) {
|
||
return Some((r.solution[0] + x_ref, r.solution[1] + y_ref));
|
||
}
|
||
|
||
// Cramer's rule fallback for the 2×2 normal equations.
|
||
let det = ata[0][0] * ata[1][1] - ata[0][1] * ata[1][0];
|
||
if det.abs() < 1e-10 {
|
||
return None;
|
||
}
|
||
let x_sol = (atb[0] * ata[1][1] - atb[1] * ata[0][1]) / det;
|
||
let y_sol = (ata[0][0] * atb[1] - ata[1][0] * atb[0]) / det;
|
||
Some((x_sol + x_ref, y_sol + y_ref))
|
||
}
|
||
|
||
#[cfg(all(test, feature = "ruvector"))]
|
||
mod triangulation_tests {
|
||
use super::*;
|
||
|
||
#[test]
|
||
fn tdoa_triangulation_insufficient_data() {
|
||
let result = solve_tdoa_triangulation(&[(0, 1, 1e-9)], &[(0.0, 0.0), (5.0, 0.0)]);
|
||
assert!(result.is_none());
|
||
}
|
||
|
||
#[test]
|
||
fn tdoa_triangulation_symmetric_case() {
|
||
// Target at centre (2.5, 2.5), APs at corners of 5m×5m square
|
||
let aps = vec![(0.0_f32, 0.0), (5.0, 0.0), (5.0, 5.0), (0.0, 5.0)];
|
||
// Target equidistant from all APs → TDoA ≈ 0 for all pairs
|
||
let measurements = vec![
|
||
(0_usize, 1_usize, 0.0_f32),
|
||
(1, 2, 0.0),
|
||
(2, 3, 0.0),
|
||
(0, 3, 0.0),
|
||
];
|
||
let result = solve_tdoa_triangulation(&measurements, &aps);
|
||
assert!(result.is_some(), "should solve symmetric case");
|
||
let (x, y) = result.unwrap();
|
||
assert!(x.is_finite() && y.is_finite());
|
||
}
|
||
}
|