locate_rs/lib.rs
1#![cfg_attr(not(test), no_std)]
2
3use heapless::{FnvIndexMap, Vec};
4use nalgebra::{ComplexField, Matrix3, RealField, RowVector3, SMatrix, SVector, Unit, Vector3};
5use num_traits::float::TotalOrder;
6use rand::RngCore;
7
8#[derive(Debug, PartialEq)]
9pub enum LocationError {
10 /// Not enough measurements or known locations to perform the calculation.
11 InsufficientData,
12 /// A node ID from the measurement data was not found in the known locations map.
13 NodeNotFound,
14 /// The iterative solver failed to converge or find a solution.
15 SolverFailed,
16 /// No real eigenvalue was found, which is necessary for the eigenvalue-based method.
17 NoRealEigenvalue,
18}
19
20/// A solver for 3D localization problems.
21///
22/// This struct provides methods to solve for a 3D position based on
23/// Time Difference of Arrival (TDOA) or trilateration data.
24pub struct LocationSolver<'a, NODE, FLOAT, const MAXNNODES: usize> {
25 /// A map of known node locations, where `NODE` is a unique identifier.
26 known_locations: &'a FnvIndexMap<NODE, Vector3<FLOAT>, MAXNNODES>,
27 /// The tolerance used to determine when an iterative solver has converged.
28 solving_tolerance: FLOAT,
29}
30
31#[allow(non_snake_case)]
32impl<'a, NODE, FLOAT, const MAXNNODES: usize> LocationSolver<'a, NODE, FLOAT, MAXNNODES>
33where
34 NODE: core::cmp::Eq,
35 NODE: core::fmt::Debug,
36 NODE: core::hash::Hash,
37 NODE: Copy,
38 FLOAT: num_traits::float::Float,
39 FLOAT: RealField,
40 FLOAT: simba::scalar::SubsetOf<f64>,
41 FLOAT: TotalOrder,
42{
43 /// Creates a new `LocationSolver`.
44 ///
45 /// # Arguments
46 ///
47 /// * `known_locations` - A reference to a map of known node positions.
48 /// * `solving_tolerance` - The tolerance for determining convergence of iterative solvers.
49 pub fn new(
50 known_locations: &FnvIndexMap<NODE, Vector3<FLOAT>, MAXNNODES>,
51 solving_tolerance: FLOAT,
52 ) -> LocationSolver<'_, NODE, FLOAT, MAXNNODES> {
53 LocationSolver {
54 known_locations,
55 solving_tolerance,
56 }
57 }
58
59 /// Solves for the location using Time Difference of Arrival (TDOA) data.
60 ///
61 /// This method uses the Levenberg-Marquardt algorithm to iteratively find the position
62 /// that best fits the given TDOA measurements.
63 ///
64 /// # Arguments
65 ///
66 /// * `tdoa_distance_infos` - A map where the key is a pair of nodes `(i, j)` and the value
67 /// is the difference in distance `d_i - d_j`. Location of `i` and `j` must be known.
68 /// * `initial_guess` - An optional initial guess for the location. If not provided,
69 /// the center of the known anchor locations is used.
70 ///
71 /// # Returns
72 ///
73 /// A `Result` containing the calculated `Vector3<FLOAT>` position, or a `LocationError` if
74 /// solving fails.
75 pub fn tdoa(
76 &mut self,
77 tdoa_distance_infos: FnvIndexMap<(NODE, NODE), FLOAT, MAXNNODES>,
78 initial_guess: Option<Vector3<FLOAT>>,
79 ) -> Result<Vector3<FLOAT>, LocationError> {
80 if tdoa_distance_infos.len() < 3 || self.known_locations.len() < 4 {
81 return Err(LocationError::InsufficientData);
82 }
83 // If no initial guess is provided, initialize x at the center of the known anchor locations.
84 let mut x = initial_guess.unwrap_or_else(|| {
85 let num_known = self.known_locations.len();
86 if num_known == 0 {
87 Vector3::zeros()
88 } else {
89 let sum_of_positions: Vector3<FLOAT> = self.known_locations.values().sum();
90 sum_of_positions / FLOAT::from_usize(num_known).unwrap()
91 }
92 });
93
94 let lambda = FLOAT::from_f64(1e-3).unwrap(); // Levenberg-Marquardt damping factor
95 let max_iterations: usize = 20;
96
97 let len_tdoa_distance_infos = tdoa_distance_infos.len();
98
99 for _ in 0..max_iterations {
100 // Populate Jacobian matrix
101 let mut residuals = SVector::<FLOAT, MAXNNODES>::zeros();
102 let mut jacobian = SMatrix::<FLOAT, MAXNNODES, 3>::zeros();
103
104 for (k, (&(i, j), &delta_d_ij)) in tdoa_distance_infos.iter().enumerate() {
105 let Pi = self
106 .known_locations
107 .get(&i)
108 .ok_or(LocationError::NodeNotFound)?;
109 let Pj = self
110 .known_locations
111 .get(&j)
112 .ok_or(LocationError::NodeNotFound)?;
113
114 let vi = x - *Pi;
115 let di = vi.norm();
116 let vj = x - *Pj;
117 let dj = vj.norm();
118
119 residuals[k] = di - dj - delta_d_ij;
120
121 // ∂r/∂x = (x - Pi)/‖x - Pi‖ - (x - Pj)/‖x - Pj‖
122 let grad = vi / di - vj / dj;
123 jacobian.row_mut(k).copy_from(&grad.transpose());
124 }
125
126 // Manually compute JᵀJ and Jᵀr to avoid allocation (or too many operations)
127 let mut jtj = Matrix3::<FLOAT>::zeros(); // JᵀJ will be a 3x3 matrix
128 let mut jtr = Vector3::<FLOAT>::zeros(); // Jᵀr will be a 3x1 vector
129
130 for k in 0..len_tdoa_distance_infos {
131 let row_k: RowVector3<FLOAT> = jacobian.row(k).into(); // Get the k-th row as a RowVector3 (1x3)
132 let res_k: FLOAT = residuals[k]; // Get the k-th residual (scalar)
133
134 // Accumulate JᵀJ: (3x1) * (1x3) = 3x3 matrix.
135 // This multiplication is SIMD-optimized by nalgebra.
136 jtj += row_k.transpose() * row_k;
137
138 // Accumulate Jᵀr: (3x1) * scalar = 3x1 vector.
139 // This multiplication is SIMD-optimized by nalgebra.
140 jtr += row_k.transpose() * res_k;
141 }
142
143 // Solve Levenberg-Marquard iteration
144 let lhs = jtj + Matrix3::identity() * lambda;
145 let rhs = -jtr;
146
147 // Solve (JᵀJ + λI) δ = -Jᵀr
148 let delta = lhs.lu().solve(&rhs).ok_or(LocationError::SolverFailed)?;
149
150 x += delta;
151
152 if delta.norm() < self.solving_tolerance {
153 break;
154 }
155 }
156
157 Ok(x)
158 }
159
160 /// Solves for the location using trilateration data with a fast iterative method.
161 ///
162 /// This method uses the Levenberg-Marquardt algorithm to iteratively find the position
163 /// that best fits the given distance measurements from known locations.
164 ///
165 /// # Arguments
166 ///
167 /// * `trilateration_infos` - A map where the key `i` is a node and the value is the
168 /// measured distance to that node. Location of node `i` must be known.
169 /// * `initial_guess` - An optional initial guess for the location. If not provided,
170 /// the center of the known anchor locations is used.
171 ///
172 /// # Returns
173 ///
174 /// A `Result` containing the calculated `Vector3<FLOAT>` position, or a `LocationError` if
175 /// solving fails.
176 pub fn trilateration_fast(
177 &mut self,
178 trilateration_infos: FnvIndexMap<NODE, FLOAT, MAXNNODES>,
179 initial_guess: Option<Vector3<FLOAT>>,
180 ) -> Result<Vector3<FLOAT>, LocationError> {
181 if trilateration_infos.len() < 4 || self.known_locations.len() < 4 {
182 return Err(LocationError::InsufficientData);
183 }
184
185 // If no initial guess is provided, initialize x at the center of the known anchor locations.
186 let mut x = initial_guess.unwrap_or_else(|| {
187 let num_known = self.known_locations.len();
188 if num_known == 0 {
189 Vector3::zeros()
190 } else {
191 let sum_of_positions: Vector3<FLOAT> = self.known_locations.values().sum();
192 sum_of_positions / FLOAT::from_usize(num_known).unwrap()
193 }
194 });
195
196 let lambda = FLOAT::from_f64(1e-3).unwrap(); // Levenberg-Marquardt damping
197 let max_iterations: usize = 50;
198
199 let len_trilateration_infos = trilateration_infos.len();
200
201 for _ in 0..max_iterations {
202 // Prepare residuals and Jacobian
203 let mut residuals = SVector::<FLOAT, MAXNNODES>::zeros();
204 let mut jacobian = SMatrix::<FLOAT, MAXNNODES, 3>::zeros();
205
206 for (k, (&node, &measured_distance)) in trilateration_infos.iter().enumerate() {
207 let anchor = self
208 .known_locations
209 .get(&node)
210 .ok_or(LocationError::NodeNotFound)?;
211 let vec = x - *anchor;
212 let dist = vec.norm();
213
214 residuals[k] = dist - measured_distance;
215
216 // ∂r/∂x = (x - anchor) / ‖x - anchor‖
217 jacobian.row_mut(k).copy_from(&(vec / dist).transpose());
218 }
219
220 // Manually compute JᵀJ and Jᵀr to avoid allocation (or too many operations)
221 let mut jtj = Matrix3::<FLOAT>::zeros(); // JᵀJ will be a 3x3 matrix
222 let mut jtr = Vector3::<FLOAT>::zeros(); // Jᵀr will be a 3x1 vector
223
224 for k in 0..len_trilateration_infos {
225 let row_k: RowVector3<FLOAT> = jacobian.row(k).into(); // Get the k-th row as a RowVector3 (1x3)
226 let res_k: FLOAT = residuals[k]; // Get the k-th residual (scalar)
227
228 // Accumulate JᵀJ: (3x1) * (1x3) = 3x3 matrix.
229 // This multiplication is SIMD-optimized by nalgebra.
230 jtj += row_k.transpose() * row_k;
231
232 // Accumulate Jᵀr: (3x1) * scalar = 3x1 vector.
233 // This multiplication is SIMD-optimized by nalgebra.
234 jtr += row_k.transpose() * res_k;
235 }
236
237 // Solve Levenberg-Marquard iteration
238 let lhs = jtj + Matrix3::identity() * lambda;
239 let rhs = -jtr;
240
241 // Solve (JᵀJ + λI) δ = -Jᵀr
242 let delta = lhs.lu().solve(&rhs).ok_or(LocationError::SolverFailed)?;
243
244 x += delta;
245
246 if delta.norm() < self.solving_tolerance {
247 break;
248 }
249 }
250
251 Ok(x)
252 }
253
254 /// Solves for the location using trilateration data with an eigenvalue-based method.
255 ///
256 /// This method is based on the paper "Optimal Trilateration is an Eigenvalue Problem"
257 /// and provides a non-iterative, closed-form solution.
258 /// See: 10.1109/icassp.2019.8683355
259 ///
260 /// # Arguments
261 ///
262 /// * `trilateration_infos` - A map where the key is a node and the value is the
263 /// measured distance to that node.
264 /// * `rng` - A random number generator for the eigenvector calculation.
265 ///
266 /// # Returns
267 ///
268 /// A `Result` containing the calculated `Vector3<FLOAT>` position, or a `LocationError` if
269 /// solving fails.
270 pub fn trilateration<RNG>(
271 &mut self,
272 trilateration_infos: FnvIndexMap<NODE, FLOAT, MAXNNODES>,
273 rng: RNG,
274 ) -> Result<Vector3<FLOAT>, LocationError>
275 where
276 RNG: RngCore,
277 {
278 // check for 3D sizes
279 if trilateration_infos.len() < 4 || self.known_locations.len() < 4 {
280 return Err(LocationError::InsufficientData);
281 }
282
283 // From here follow 10.1109/icassp.2019.8683355, Chapter 1.4, 2.1, 2.2
284 // get all location, distance^2, weight pairs (weight as in (6) of the paper)
285 let mut data: Vec<(Vector3<FLOAT>, FLOAT, FLOAT), MAXNNODES> = Vec::new();
286 for (node, distance) in trilateration_infos {
287 let _ = data.push((
288 *self
289 .known_locations
290 .get(&node)
291 .ok_or(LocationError::NodeNotFound)?,
292 ComplexField::powi(distance, 2),
293 FLOAT::one() / (FLOAT::from(4.0).unwrap() * ComplexField::powi(distance, 2)),
294 ));
295 }
296
297 // calculate t as translation value
298 let mut w_sum = FLOAT::zero();
299 let mut t: Vector3<FLOAT> = Vector3::zeros();
300 for (p_i, _, w_i) in &data {
301 w_sum += *w_i;
302 t += p_i.map(|x| x * *w_i);
303 }
304 t = -(t / w_sum);
305
306 // Compute matrix A and b
307 let mut A: Matrix3<FLOAT> = Matrix3::zeros();
308 let mut b: Vector3<FLOAT> = Vector3::zeros();
309
310 for (p, d_pow2, mut w) in data.clone() {
311 // normalize w
312 w /= w_sum;
313
314 // translate s
315 let s: Vector3<FLOAT> = p + t;
316
317 // Compute w*((s' * s) - d^2) which results in a scalar (1x1 matrix)
318 let w_s_ts_minus_dpow2 = w * ((s.transpose() * s)[(0, 0)] - d_pow2);
319
320 // Update matrix A (remember, s_ts is a 1x1 matrix, so extract its scalar value with `[(0, 0)]`)
321 A += Matrix3::from_diagonal_element(w_s_ts_minus_dpow2)
322 + (s * s.transpose()).map(|x| w * FLOAT::from(2.0).unwrap() * x);
323 // Update vector b
324 b += s.map(|x| x * -w_s_ts_minus_dpow2);
325 }
326 // Compute matrix D, containing eigenvalues of A
327 let A_decomp = A.symmetric_eigen();
328 let U = A_decomp.eigenvectors;
329 let D = Matrix3::from_diagonal(&A_decomp.eigenvalues);
330
331 // Transform b
332 b = U.transpose() * b;
333
334 // Generate M
335 let mut M: SMatrix<FLOAT, 7, 7> = SMatrix::zeros(); // Initialize a 9x9 matrix of zeros
336 // Fill in the blocks
337 M.view_mut((0, 0), (3, 3)).copy_from(&-D); // -D
338 M.view_mut((0, 3), (3, 3))
339 .copy_from(&-Matrix3::from_diagonal(&b));
340 M.view_mut((3, 3), (3, 3)).copy_from(&-D); // -D
341 M.view_mut((3, 6), (3, 1)).copy_from(&-b); // -b
342 M.view_mut((6, 0), (1, 3))
343 .copy_from(&RowVector3::repeat(FLOAT::one())); // identity
344
345 // Get eigenvectors of M corresponding to largest real eigenvalue,
346 // APPARENTLY: MAX EIGENVALUE CORRESPONDS TO THE OPTIMUM SOLUTION.. WHY? I DON'T KNOW!
347 let max_real_eigenvalue = M
348 .complex_eigenvalues()
349 .into_iter()
350 .filter(|lambda| lambda.im == FLOAT::zero())
351 .max_by(|a, b| a.re.total_cmp(&b.re))
352 .ok_or(LocationError::NoRealEigenvalue)?
353 .re;
354 let mut x: SVector<FLOAT, 7> = self.get_ev(&M, max_real_eigenvalue, rng)?;
355 // scale them by last value,
356 x /= x[(6, 0)];
357 // get elements 3:5
358 let mut x = x.remove_row(6).remove_row(0).remove_row(0).remove_row(0);
359 // transform to U*x - t
360 x = U * x - t;
361
362 // Return computed solution
363 Ok(x)
364 }
365
366 /// Computes the eigenvector for a given eigenvalue using inverse iteration.
367 ///
368 /// This is a helper function for `trilateration` where the eigenvector of the
369 /// largest eigenvalue is searched.
370 ///
371 /// # Arguments
372 ///
373 /// * `A` - The matrix for which to find the eigenvector.
374 /// * `lambda` - The eigenvalue corresponding to the desired eigenvector.
375 /// * `rng` - A random number generator to initialize the process.
376 ///
377 /// # Returns
378 ///
379 /// A `Result` containing the calculated eigenvector, or `Err(())` if solving fails.
380 #[allow(non_snake_case)]
381 fn get_ev<RNG>(
382 &mut self,
383 A: &SMatrix<FLOAT, 7, 7>,
384 lambda: FLOAT,
385 mut rng: RNG,
386 ) -> Result<SVector<FLOAT, 7>, LocationError>
387 where
388 RNG: RngCore,
389 {
390 // create 1e-3 deviation of lambda, s.t. matrix to be inversed is non-singular (works better with f64)
391 let mu = lambda - FLOAT::from(1e-3).unwrap();
392
393 // Generate matrix M
394 let M = A - SMatrix::<FLOAT, 7, 7>::from_diagonal_element(mu);
395 // Get LU transform to solve M^-1 * b
396 let M = M.lu();
397
398 let mut b = Unit::new_normalize(
399 SVector::<FLOAT, 7>::zeros()
400 .map(|_| FLOAT::from(rng.next_u32()).unwrap() / FLOAT::from(u32::MAX).unwrap()),
401 );
402
403 // Should converge pretty fast, anyway restrict iterations by 1000 - most of the time it will take less time
404 for _ in 0..1000 {
405 let b_new: Unit<SVector<FLOAT, 7>> =
406 Unit::new_normalize(M.solve(&b).ok_or(LocationError::SolverFailed)?);
407 if (*b_new - *b).norm() < self.solving_tolerance {
408 return Ok(*b);
409 }
410 b = b_new;
411 }
412 Ok(*b)
413 }
414}