All files / src/core/utils multilateration.ts

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
export function trilaterateNLLS(beacons: {
  position: { x: number; y: number };
  rssi: number;
}[]): { x: number; y: number } {
  Iif (beacons.length < 3) throw new Error("At least 3 beacons required");
 
  const txPower = -65;
  const pathLossExp = 1.8;
 
  const processedBeacons = beacons.map(b => {
 
    const distance = Math.pow(10, (txPower - b.rssi) / (10 * pathLossExp));
    console.log('distance:', distance);
    return {
      position: b.position,
      distance
    };
  });
 
  // Initial guess: centroid
  let x = processedBeacons.reduce((sum, b) => sum + b.position.x, 0) / processedBeacons.length;
  let y = processedBeacons.reduce((sum, b) => sum + b.position.y, 0) / processedBeacons.length;
 
  let learningRate = 0.1;
  const MAX_ITER = 500;
  const TOLERANCE = 1e-6;
 
  for (let iter = 0; iter < MAX_ITER; iter++) {
    let gradX = 0;
    let gradY = 0;
    let totalError = 0;
 
    for (const beacon of processedBeacons) {
      const dx = x - beacon.position.x;
      const dy = y - beacon.position.y;
      const dist = Math.sqrt(dx * dx + dy * dy) + 1e-6;
 
      const diff = dist - beacon.distance;
      totalError += diff * diff;
 
      const weight = 1 / (beacon.distance + 1e-6);
      gradX += weight * (diff * dx) / dist;
      gradY += weight * (diff * dy) / dist;
    }
 
    x -= learningRate * gradX;
    y -= learningRate * gradY;
 
    Iif (totalError < TOLERANCE || (Math.abs(gradX) < 1e-8 && Math.abs(gradY) < 1e-8)) {
      break;
    }
  }
 
  return { x, y };
}