sensor_fusion/
trilaterate_2d.rs

1use core::ops::{Add, AddAssign};
2use num_traits::float::FloatCore;
3use vqm::{Matrix2x2, Vector2};
4
5/// 2-dimensional anchor of `f32` values<br><br>
6pub type Anchor2df32 = Anchor2d<f32>;
7/// 2-dimensional anchor of `f64` values<br><br>
8pub type Anchor2df64 = Anchor2d<f64>;
9
10#[derive(Clone, Copy, Debug, PartialEq)]
11//#[cfg_attr(feature = "std", derive(derive_more::Display))]
12//#[cfg_attr(feature = "std", display("V{{x:{x}, y:{y}}}"))]
13pub struct Anchor2d<T> {
14    pub pos: Vector2<T>,
15    pub distance: T,
16}
17
18impl<T> Default for Anchor2d<T>
19where
20    T: FloatCore,
21{
22    fn default() -> Self {
23        Self::new(T::zero(), T::zero())
24    }
25}
26
27impl<T> Anchor2d<T>
28where
29    T: FloatCore,
30{
31    /// Constructor.
32    #[must_use]
33    pub fn new(x: T, y: T) -> Self {
34        Anchor2d { pos: Vector2 { x, y }, distance: T::zero() }
35    }
36}
37
38/// Trilaterates a 2d position using the least squares method.
39#[must_use]
40pub fn trilaterate_2d<T>(anchors: &[Anchor2d<T>]) -> Option<Vector2<T>>
41where
42    T: Default + FloatCore + Add<T, Output = T> + AddAssign + vqm::Matrix2x2Math,
43{
44    if anchors.len() < 3 {
45        return None;
46    }
47
48    // Find the closest anchor and use it as the base anchor.
49    let mut base_idx = 0;
50    let mut min_distance = anchors[0].distance;
51
52    for (ii, anchor) in anchors.iter().enumerate().skip(1) {
53        if anchor.distance < min_distance {
54            min_distance = anchor.distance;
55            base_idx = ii;
56        }
57    }
58
59    let base = anchors[base_idx];
60    let base_norm_squared = base.pos.norm_squared();
61    let base_distance_squared = base.distance * base.distance;
62
63    // Initialize the accumulation structure for the 2x2 matrix ((A^T * A))
64    // Since A^T * A is symmetric we only need to calculate the upper right corner of the matrix.
65    // [ ata_00  ata_01 ]
66    // [         ata_11 ]
67    let mut ata_00 = T::zero();
68    let mut ata_01 = T::zero();
69    let mut ata_11 = T::zero();
70
71    // Initialize the 2x1 vector (atb)
72    let mut atb = Vector2::default();
73
74    // Accumulate all rows, skipping the base anchor
75    for (ii, anchor) in anchors.iter().enumerate() {
76        if ii == base_idx {
77            continue;
78        }
79        // Calculate row components for Matrix A
80        let a_row = (base.pos - anchor.pos) * (T::one() + T::one());
81
82        // Accumulate into the (A^T * W * A) matrix
83        ata_00 += a_row.x * a_row.x;
84        ata_01 += a_row.x * a_row.y;
85        ata_11 += a_row.y * a_row.y;
86
87        // Calculate vector B row values
88        let b_row = (anchor.distance * anchor.distance - base_distance_squared)
89            - (anchor.pos.norm_squared() - base_norm_squared);
90        // Accumulate into the 3d vector A^T * B elements
91        atb += a_row * b_row;
92    }
93
94    let m = Matrix2x2::new([
95        ata_00, ata_01, //
96        ata_01, ata_11, //
97    ]);
98
99    let (adjugate, determinant) = m.adjugate();
100    if determinant.abs() < T::min_positive_value() { None } else { Some((adjugate * atb) / determinant) }
101}
102
103#[cfg(test)]
104mod tests {
105    use super::*;
106
107    fn _is_normal<T: Sized + Send + Sync + Unpin>() {}
108    fn is_full<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + PartialEq>() {}
109
110    #[test]
111    fn normal_types() {
112        is_full::<Anchor2d<f32>>();
113    }
114}
sensor_fusion/trilaterate_2d.rs

sensor_fusion/
trilaterate_2d.rs
