1#![cfg_attr(not(feature = "std"), no_std)]
2
3#[cfg(not(feature = "std"))]
4extern crate alloc;
5
6#[cfg(not(feature = "std"))]
7use alloc::{string::String, vec::Vec};
8
9use bincode::{Decode, Encode};
10use cu_sensor_payloads::{PeerRangeSample, PeerRangeSnapshot, RangePeerId};
11use cu29::prelude::*;
12use cu29::units::si::f32::Length;
13use cu29::units::si::length::meter;
14use serde::{Deserialize, Serialize};
15
16#[derive(
17 Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect,
18)]
19pub struct LocalPosition3d {
20 pub x: Length,
21 pub y: Length,
22 pub z: Length,
23 pub rms_residual: Length,
24}
25
26impl LocalPosition3d {
27 pub fn from_meters(x_m: f32, y_m: f32, z_m: f32, rms_residual_m: f32) -> Self {
28 Self {
29 x: Length::new::<meter>(x_m),
30 y: Length::new::<meter>(y_m),
31 z: Length::new::<meter>(z_m),
32 rms_residual: Length::new::<meter>(rms_residual_m),
33 }
34 }
35}
36
37#[derive(
38 Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect,
39)]
40pub struct PeerAnchor3d {
41 pub peer_id: RangePeerId,
42 pub x: Length,
43 pub y: Length,
44 pub z: Length,
45}
46
47impl PeerAnchor3d {
48 pub fn from_meters(peer_id: RangePeerId, x_m: f32, y_m: f32, z_m: f32) -> Self {
49 Self {
50 peer_id,
51 x: Length::new::<meter>(x_m),
52 y: Length::new::<meter>(y_m),
53 z: Length::new::<meter>(z_m),
54 }
55 }
56
57 fn x_m(self) -> f32 {
58 self.x.get::<meter>()
59 }
60
61 fn y_m(self) -> f32 {
62 self.y.get::<meter>()
63 }
64
65 fn z_m(self) -> f32 {
66 self.z.get::<meter>()
67 }
68}
69
70#[derive(Debug, Deserialize)]
71struct AnchorConfig {
72 peer_id: String,
73 x_m: f32,
74 y_m: f32,
75 z_m: f32,
76}
77
78#[derive(Reflect)]
79pub struct PeerTriangulation3dTask<const SNAPSHOT_N: usize, const ANCHOR_N: usize> {
80 anchors: [Option<PeerAnchor3d>; ANCHOR_N],
81 len: usize,
82 max_rms_residual: Option<Length>,
83}
84
85impl<const SNAPSHOT_N: usize, const ANCHOR_N: usize> Freezable
86 for PeerTriangulation3dTask<SNAPSHOT_N, ANCHOR_N>
87{
88}
89
90impl<const SNAPSHOT_N: usize, const ANCHOR_N: usize> PeerTriangulation3dTask<SNAPSHOT_N, ANCHOR_N> {
91 pub fn new_with_anchors(
92 anchors: &[PeerAnchor3d],
93 max_rms_residual: Option<Length>,
94 ) -> Result<Self, PeerTriangulationError> {
95 if anchors.len() > ANCHOR_N {
96 return Err(PeerTriangulationError::TooManyAnchors {
97 len: anchors.len(),
98 capacity: ANCHOR_N,
99 });
100 }
101
102 let mut task = Self {
103 anchors: [None; ANCHOR_N],
104 len: anchors.len(),
105 max_rms_residual,
106 };
107 for (slot, anchor) in task.anchors.iter_mut().zip(anchors) {
108 *slot = Some(*anchor);
109 }
110
111 Ok(task)
112 }
113
114 pub fn estimate(
115 &self,
116 snapshot: &PeerRangeSnapshot<SNAPSHOT_N>,
117 ) -> Result<Option<LocalPosition3d>, PeerTriangulationError> {
118 let mut matched = [None; SNAPSHOT_N];
119 let mut matched_len = 0;
120
121 for sample in snapshot.samples() {
122 if let Some(anchor) = self.anchor_for(sample) {
123 matched[matched_len] = Some((*sample, anchor));
124 matched_len += 1;
125 }
126 }
127
128 if matched_len < 4 {
129 return Ok(None);
130 }
131
132 let Some((reference_sample, reference_anchor)) = matched[0] else {
133 return Ok(None);
134 };
135 let x0 = reference_anchor.x_m();
136 let y0 = reference_anchor.y_m();
137 let z0 = reference_anchor.z_m();
138 let r0 = reference_sample.observation.distance.get::<meter>();
139
140 let mut ata = [[0.0_f32; 3]; 3];
141 let mut atb = [0.0_f32; 3];
142
143 for matched in matched[1..matched_len].iter().flatten() {
144 let (sample, anchor) = *matched;
145 let xi = anchor.x_m();
146 let yi = anchor.y_m();
147 let zi = anchor.z_m();
148 let ri = sample.observation.distance.get::<meter>();
149 let row = [2.0 * (xi - x0), 2.0 * (yi - y0), 2.0 * (zi - z0)];
150 let b =
151 (r0 * r0 - ri * ri) - (x0 * x0 + y0 * y0 + z0 * z0) + (xi * xi + yi * yi + zi * zi);
152
153 for i in 0..3 {
154 atb[i] += row[i] * b;
155 for j in 0..3 {
156 ata[i][j] += row[i] * row[j];
157 }
158 }
159 }
160
161 let Some([x, y, z]) = solve_3x3(ata, atb) else {
162 return Err(PeerTriangulationError::DegenerateGeometry);
163 };
164 let rms_residual = self.rms_residual_m(&matched[..matched_len], x, y, z);
165
166 if let Some(max_rms_residual) = self.max_rms_residual
167 && rms_residual > max_rms_residual.get::<meter>()
168 {
169 return Ok(None);
170 }
171
172 Ok(Some(LocalPosition3d::from_meters(x, y, z, rms_residual)))
173 }
174
175 fn anchor_for(&self, sample: &PeerRangeSample) -> Option<PeerAnchor3d> {
176 self.anchors[..self.len]
177 .iter()
178 .flatten()
179 .copied()
180 .find(|anchor| anchor.peer_id == sample.observation.peer_id)
181 }
182
183 fn rms_residual_m(
184 &self,
185 matched: &[Option<(PeerRangeSample, PeerAnchor3d)>],
186 x: f32,
187 y: f32,
188 z: f32,
189 ) -> f32 {
190 let mut sum_sq = 0.0_f32;
191 let mut count = 0_u32;
192
193 for (sample, anchor) in matched.iter().flatten() {
194 let dx = x - anchor.x_m();
195 let dy = y - anchor.y_m();
196 let dz = z - anchor.z_m();
197 let expected = libm::sqrtf(dx * dx + dy * dy + dz * dz);
198 let residual = expected - sample.observation.distance.get::<meter>();
199 sum_sq += residual * residual;
200 count += 1;
201 }
202
203 libm::sqrtf(sum_sq / count as f32)
204 }
205}
206
207impl<const SNAPSHOT_N: usize, const ANCHOR_N: usize> CuTask
208 for PeerTriangulation3dTask<SNAPSHOT_N, ANCHOR_N>
209{
210 type Resources<'r> = ();
211 type Input<'m> = input_msg!(PeerRangeSnapshot<SNAPSHOT_N>);
212 type Output<'m> = output_msg!(LocalPosition3d);
213
214 fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
215 where
216 Self: Sized,
217 {
218 let config = config.ok_or("peer triangulation task requires config")?;
219 let anchors = config
220 .get_value::<Vec<AnchorConfig>>("anchors")?
221 .ok_or("peer triangulation task requires anchors")?;
222 let mut parsed = [PeerAnchor3d::default(); ANCHOR_N];
223 let anchor_len = anchors.len();
224 if anchor_len < 4 {
225 return Err(CuError::from(
226 "peer triangulation config must define at least 4 anchors",
227 ));
228 }
229 if anchor_len > ANCHOR_N {
230 return Err(CuError::from(
231 "peer triangulation config has too many anchors",
232 ));
233 }
234 for (slot, anchor) in parsed.iter_mut().zip(anchors) {
235 *slot = PeerAnchor3d::from_meters(
236 RangePeerId::new(anchor.peer_id.as_str())
237 .map_err(|_| CuError::from("peer triangulation config has invalid peer id"))?,
238 anchor.x_m,
239 anchor.y_m,
240 anchor.z_m,
241 );
242 }
243
244 let max_rms_residual = config
245 .get::<f64>("max_rms_residual_m")?
246 .map(|value| Length::new::<meter>(value as f32));
247
248 Self::new_with_anchors(&parsed[..anchor_len], max_rms_residual)
249 .map_err(|_| CuError::from("peer triangulation config has too many anchors"))
250 }
251
252 fn process(
253 &mut self,
254 _ctx: &CuContext,
255 input: &Self::Input<'_>,
256 output: &mut Self::Output<'_>,
257 ) -> CuResult<()> {
258 let Some(snapshot) = input.payload() else {
259 output.clear_payload();
260 return Ok(());
261 };
262
263 match self
264 .estimate(snapshot)
265 .map_err(|_| CuError::from("peer triangulation geometry is degenerate"))?
266 {
267 Some(position) => {
268 output.tov = input.tov;
269 output.set_payload(position);
270 }
271 None => output.clear_payload(),
272 }
273
274 Ok(())
275 }
276}
277
278#[derive(Clone, Copy, Debug, Eq, PartialEq)]
279pub enum PeerTriangulationError {
280 TooManyAnchors { len: usize, capacity: usize },
281 DegenerateGeometry,
282}
283
284impl core::fmt::Display for PeerTriangulationError {
285 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
286 match self {
287 Self::TooManyAnchors { len, capacity } => {
288 write!(f, "{len} anchors exceed capacity {capacity}")
289 }
290 Self::DegenerateGeometry => write!(f, "peer range geometry is degenerate"),
291 }
292 }
293}
294
295impl core::error::Error for PeerTriangulationError {}
296
297fn solve_3x3(a: [[f32; 3]; 3], b: [f32; 3]) -> Option<[f32; 3]> {
298 let det = determinant_3x3(a);
299 if det.abs() <= f32::EPSILON {
300 return None;
301 }
302
303 let mut ax = a;
304 ax[0][0] = b[0];
305 ax[1][0] = b[1];
306 ax[2][0] = b[2];
307
308 let mut ay = a;
309 ay[0][1] = b[0];
310 ay[1][1] = b[1];
311 ay[2][1] = b[2];
312
313 let mut az = a;
314 az[0][2] = b[0];
315 az[1][2] = b[1];
316 az[2][2] = b[2];
317
318 Some([
319 determinant_3x3(ax) / det,
320 determinant_3x3(ay) / det,
321 determinant_3x3(az) / det,
322 ])
323}
324
325fn determinant_3x3(a: [[f32; 3]; 3]) -> f32 {
326 a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1])
327 - a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0])
328 + a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0])
329}
330
331#[cfg(test)]
332mod tests {
333 use super::*;
334 use cu_sensor_payloads::{PeerRangeObservation, RangePeerId};
335 use cu29::units::si::length::meter;
336
337 fn anchor(peer_id: &str, x: f32, y: f32, z: f32) -> PeerAnchor3d {
338 PeerAnchor3d::from_meters(RangePeerId::new(peer_id).unwrap(), x, y, z)
339 }
340
341 fn sample(peer_id: &str, meters: f32) -> PeerRangeSample {
342 PeerRangeSample::new(
343 CuTime::from_nanos(1),
344 PeerRangeObservation::from_meters(RangePeerId::new(peer_id).unwrap(), meters, None),
345 )
346 }
347
348 #[test]
349 fn estimates_exact_position_from_four_ranges() {
350 let task = PeerTriangulation3dTask::<5, 5>::new_with_anchors(
351 &[
352 anchor("A", 0.0, 0.0, 0.0),
353 anchor("B", 4.0, 0.0, 0.0),
354 anchor("C", 0.0, 3.0, 0.0),
355 anchor("D", 0.0, 0.0, 4.0),
356 ],
357 None,
358 )
359 .unwrap();
360 let mut snapshot = PeerRangeSnapshot::<5>::new();
361 snapshot.push(sample("A", 2.449_489_8)).unwrap();
362 snapshot.push(sample("B", 2.449_489_8)).unwrap();
363 snapshot.push(sample("C", 3.0)).unwrap();
364 snapshot.push(sample("D", 3.741_657_5)).unwrap();
365
366 let position = task.estimate(&snapshot).unwrap().unwrap();
367
368 assert!((position.x.get::<meter>() - 2.0).abs() < 0.001);
369 assert!((position.y.get::<meter>() - 1.0).abs() < 0.001);
370 assert!((position.z.get::<meter>() - 1.0).abs() < 0.001);
371 assert!(position.rms_residual.get::<meter>() < 0.001);
372 }
373
374 #[test]
375 fn returns_none_when_too_few_ranges_match_anchors() {
376 let task = PeerTriangulation3dTask::<5, 5>::new_with_anchors(
377 &[
378 anchor("A", 0.0, 0.0, 0.0),
379 anchor("B", 4.0, 0.0, 0.0),
380 anchor("C", 0.0, 3.0, 0.0),
381 anchor("D", 0.0, 0.0, 4.0),
382 ],
383 None,
384 )
385 .unwrap();
386 let mut snapshot = PeerRangeSnapshot::<5>::new();
387 snapshot.push(sample("A", 2.0)).unwrap();
388 snapshot.push(sample("B", 2.0)).unwrap();
389 snapshot.push(sample("C", 2.0)).unwrap();
390
391 assert!(task.estimate(&snapshot).unwrap().is_none());
392 }
393
394 #[test]
395 fn rejects_coplanar_anchor_geometry() {
396 let task = PeerTriangulation3dTask::<5, 5>::new_with_anchors(
397 &[
398 anchor("A", 0.0, 0.0, 0.0),
399 anchor("B", 1.0, 0.0, 0.0),
400 anchor("C", 0.0, 1.0, 0.0),
401 anchor("D", 1.0, 1.0, 0.0),
402 ],
403 None,
404 )
405 .unwrap();
406 let mut snapshot = PeerRangeSnapshot::<5>::new();
407 snapshot.push(sample("A", 1.0)).unwrap();
408 snapshot.push(sample("B", 1.0)).unwrap();
409 snapshot.push(sample("C", 1.0)).unwrap();
410 snapshot.push(sample("D", 1.0)).unwrap();
411
412 assert_eq!(
413 task.estimate(&snapshot),
414 Err(PeerTriangulationError::DegenerateGeometry)
415 );
416 }
417
418 #[test]
419 fn residual_gate_suppresses_noisy_estimate() {
420 let task = PeerTriangulation3dTask::<5, 5>::new_with_anchors(
421 &[
422 anchor("A", 0.0, 0.0, 0.0),
423 anchor("B", 4.0, 0.0, 0.0),
424 anchor("C", 0.0, 3.0, 0.0),
425 anchor("D", 0.0, 0.0, 4.0),
426 ],
427 Some(Length::new::<meter>(0.01)),
428 )
429 .unwrap();
430 let mut snapshot = PeerRangeSnapshot::<5>::new();
431 snapshot.push(sample("A", 2.449_489_8)).unwrap();
432 snapshot.push(sample("B", 2.449_489_8)).unwrap();
433 snapshot.push(sample("C", 3.0)).unwrap();
434 snapshot.push(sample("D", 4.5)).unwrap();
435
436 assert!(task.estimate(&snapshot).unwrap().is_none());
437 }
438}