# Position triangulation of moving nodes

Traditional wireless triangulation works by already knowing the position of 3 point and calculating the position we are looking for using these 3 points.

I want to calculate the relative position of my nodes in a network where they are moving. There can be 3+ nodes seeing each other at once. All I have is a signal strength which might vary depending on the environnement(signal strength is not a linear function of distance.)

What are the mathematical concepts that can help me to estimate the relative position of each node to each other?

• Please clarify what it is you know and what it is you wish to calculate based on that knowledge. – Jens Apr 30 '17 at 20:20

At first, it is necessary to provide and determine a single characteristic "Distance - Signal level" $w(d)$ for all pairs of sensors. That allows to get the distances $$d_{12} = M_1M_2,\quad d_{23} = M_2M_3,\quad d_{31} = M_1M_3,$$ which are the sides of the triangle $\ M_1M_2M_3.$
The angles of this triangle can be calculated on the basis of the cosine theorem.

The desired order of the points $\ M_1,\ M_2,\ M_3\$ is in the counterclockwise direction. That allows to form the polar coordinates system with the positive angle from vector $\ \overrightarrow{M_1M_2}\$ to $\ \overrightarrow{M_1M_3}.\$

In this way, the polar coordinates of points $\ M_1,\ M_2,\ M_3\$ are $$M_1(0, 0),\quad M_2(d_{12}, 0),\quad M_3(d_{13}, \varphi_3),$$ where $$\varphi_3 = \arccos\dfrac{d_{12}^2 + d_{13}^2 - d_{23}^2}{2d_{12}d_{13}}.$$ And these points can be used to bind the remaining points.

For example, the point $M_4$ with the distances
$$M_1M_4 = d_{14},\quad M_2M_4 = d_{24},\quad M_3M_4 = d_{34},$$ have the polar coordinates $$M_4(d_4,\varphi_4),$$ where
$$|\varphi_4| = \arccos\dfrac{d_{12}^2 + d_{14}^2 - d_{24}^2}{2d_{12}d_{14}},\\ (d_{13}\cos\varphi_3 - d_{14}\cos\varphi_4)^2+(d_{13}\sin\varphi_3 - d_{14}\sin\varphi_4)^2 = d_{34}^2.$$

Data Smoothing

Data smoothing seems as a nesessary preparing part of the algorithm and must be applied independently to the data series of each distance. For this case can be recommended the median smoothing, which leads to a reduction in both static and dynamic measurement errors and is the optimal way of signal data processing in the presence of impulse noise.

The author's translation of the post is given below.

The median processing algorithm in the sliding window replaces the $i$-th time element of the series by the median value among the elements with the numbers $(i - h, i + h),$ where $h$ is the half-width of the window (the recommended values are $h = 3 ... 4$).

The processing is effective at a high level of impulse noise (in the test example, a third of the data is distorted). An additional advantage is that the format of the original data is preserved. Edge processing is carried out on smaller windows.

The processing faults in the sliding window are manifested during the reversal of the sequence, since the protrusions and dips of width less than $h$ are flattened out.

In the demo program, a recursive sorting of the array in a sliding window is presented, implemented based on the sorting of the inserts. For this purpose, the points lying between the old (deleted) and the new (added) elements are shifted towards the old element, after which a new element is written into the place of the last duplicate that arises. The algorithm obtained has a speed of at least $O(h)$ per the data element.

Demo program (PHP):

function print_a($a,$name){
print("$name: "); foreach($a as $item){ printf("%2d, ",$item);
}
}

function slide_median($h,$a){
$size = count($a);
$result = [];$slide = [];
array_push($slide, reset($a));
array_push($result,$slide[0]);
print_a($slide, "&emsp;Sorting in the window"); print_a($result, "<br>Resulting array");

for($i=1;$i<=$h;$i++){
array_push($slide, next($a), next($a)); sort($slide);
array_push($result,$slide[$i]); print_a($slide, "&emsp;Sorting in the window");
print_a($result, "<br>Resulting array"); } for($i=0; $i <$size-2*$h-1;$i++){
$old =$a[$i];$new = $a[$i+2*$h+1]; if($old < $new){ for($key = 0; $key <= 2*$h; $key++){ if($new < $slide[$key]){
break;
}
if(($old <=$slide[$key])&&($slide[$key] <$new)) $slide[$key] = $slide[$key+1];
}
$slide[$key-1] = $new; } if($old > $new){ for($key = 2*$h;$key >= 0; $key--){ if($new > $slide[$key]){
break;
}
if(($old >=$slide[$key])&&($slide[$key] >$new)) $slide[$key] = $slide[$key-1];
}
$slide[$key+1] = $new; } array_push($result, $slide[$h]);
print("&emsp;old = $old, new =$new");
print_a($slide, "&emsp;Sorting in the window"); print_a($result, "<br>Resulting array");
}

for($i =$h-1; $i > 0;$i--){
$slide = array_slice($a, $size-2*$i-1, 2*$i+1); sort($slide);
array_push($result,$slide[$i]); print_a($slide, "&emsp;Sorting in the window");
print_a($result, "<br>Resulting array"); }$slide = [$a[$size-1]];
array_push($result,$slide[0]);
print_a([end($a)], "&emsp;Sorting in the window"); print_a($a, "<br><br>Issue array: ");
print_a($result, "<br>Resulting array"); return$result;
};

$a = range(20, 40); foreach($a as &$item){$item += 5*mt_rand(-1,1)*(int)(mt_rand(0,199)/100);
}
print_a($a, "Issue array: "); slide_median(3,$a);


Results (impulse noise, amplitude 5):

Issue array: 20, 21, 22, 23, 19, 20, 21, 27, 28, 29, 30, 31, 32, 28, 29, 35, 36, 42, 43, 39, 35,  Sorting in the window: 20,
Resulting array: 20,  Sorting in the window: 20, 21, 22,
Resulting array: 20, 21,  Sorting in the window: 19, 20, 21, 22, 23,
Resulting array: 20, 21, 21,  Sorting in the window: 19, 20, 20, 21, 21, 22, 23,
Resulting array: 20, 21, 21, 21,  old = 20, new =27 Sorting in the window: 19, 20, 21, 21, 22, 23, 27,
Resulting array: 20, 21, 21, 21, 21,  old = 21, new =28 Sorting in the window: 19, 20, 21, 22, 23, 27, 28,
Resulting array: 20, 21, 21, 21, 21, 22,  old = 22, new =29 Sorting in the window: 19, 20, 21, 23, 27, 28, 29,
Resulting array: 20, 21, 21, 21, 21, 22, 23,  old = 23, new =30 Sorting in the window: 19, 20, 21, 27, 28, 29, 30,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27,  old = 19, new =31 Sorting in the window: 20, 21, 27, 28, 29, 30, 31,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28,  old = 20, new =32 Sorting in the window: 21, 27, 28, 29, 30, 31, 32,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29,  old = 21, new =28 Sorting in the window: 27, 28, 28, 29, 30, 31, 32,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29,  old = 27, new =29 Sorting in the window: 28, 28, 29, 29, 30, 31, 32,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29,  old = 28, new =35 Sorting in the window: 28, 29, 29, 30, 31, 32, 35,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30,  old = 29, new =36 Sorting in the window: 28, 29, 30, 31, 32, 35, 36,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31,  old = 30, new =42 Sorting in the window: 28, 29, 31, 32, 35, 36, 42,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31, 32,  old = 31, new =43 Sorting in the window: 28, 29, 32, 35, 36, 42, 43,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31, 32, 35,  old = 32, new =39 Sorting in the window: 28, 29, 35, 36, 39, 42, 43,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31, 32, 35, 36,  old = 28, new =35 Sorting in the window: 29, 35, 35, 36, 39, 42, 43,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31, 32, 35, 36, 36,  Sorting in the window: 35, 36, 39, 42, 43,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31, 32, 35, 36, 36, 39,  Sorting in the window: 35, 39, 43,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31, 32, 35, 36, 36, 39, 39,  Sorting in the window: 35,

Issue array: : 20, 21, 22, 23, 19, 20, 21, 27, 28, 29, 30, 31, 32, 28, 29, 35, 36, 42, 43, 39, 35,
Resulting array: 20, 21, 21, 21, 21, 22, 23, 27, 28, 29, 29, 29, 30, 31, 32, 35, 36, 36, 39, 39, 35,


A comparison of the slide medians and the moving average on an intense impulse noise was performed using the following program:

function print_a($a,$name){
print("$name: "); foreach($a as $item){ printf("%3d, ",$item);
}
}

function slide_median($h,$a){
$size = count($a);
$result = [];$slide = [];
array_push($slide, reset($a));
array_push($result,$slide[0]);

for($i=1;$i<=$h;$i++){
array_push($slide, next($a), next($a)); sort($slide);
array_push($result,$slide[$i]); } for($i=0; $i <$size-2*$h-1;$i++){
$old =$a[$i];$new = $a[$i+2*$h+1]; if($old < $new){ for($key = 0; $key <= 2*$h; $key++){ if($new < $slide[$key]){
break;
}
if(($old <=$slide[$key])&&($slide[$key] <$new)) $slide[$key] = $slide[$key+1];
}
$slide[$key-1] = $new; } if($old > $new){ for($key = 2*$h;$key >= 0; $key--){ if($new > $slide[$key]){
break;
}
if(($old >=$slide[$key])&&($slide[$key] >$new)) $slide[$key] = $slide[$key-1];
}
$slide[$key+1] = $new; } array_push($result, $slide[$h]);
}

for($i =$h-1; $i > 0;$i--){
$slide = array_slice($a, $size-2*$i-1, 2*$i+1); sort($slide);
array_push($result,$slide[$i]); }$slide = [$a[$size-1]];
array_push($result,$slide[0]);
print_a($a, "<br><br>Issue array "); print_a($result, "<br>Slide medians &emsp;");

return $result; }; function slide_average($h, $a){$size = count($a);$b = array_merge([0], $a);$sum = reset($a);$result = [$sum]; for($i=1; $i<=$h; $i++){$sum += next($a)+next($a);
$average = (int)($sum/(2*$i+1)+.5); array_push($result, $average); } reset($b);
for($i=0;$i < $size-2*$h-1; $i++){$sum += next($a) - next($b);
$average = (int)($sum/(2*$h+1)+.5); array_push($result, $average); } for($i = $h-1;$i >=0; $i--){$sum -= (next($b) + next($b));
$average = (int)($sum/(2*$i+1)+.5); array_push($result, $average); } print_a($a, "<br><br>Issue array ");
print_a($result, "<br>Moving average &ensp;"); return$result;
};

$a = range(200, 240); foreach($a as &$item){$item += 50*mt_rand(-1,1)*(int)(mt_rand(0,149)/100);
}
slide_median(3, $a); slide_average(3,$a);


Results:

Issue array : 200, 201, 202, 203, 204, 155, 206, 207, 208, 209, 210, 211, 212, 213, 264, 265, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 183, 234, 235, 236, 287, 238, 239, 240,
Slide medians  : 200, 201, 202, 202, 203, 204, 206, 207, 208, 209, 210, 211, 212, 213, 216, 217, 218, 219, 219, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 229, 230, 231, 232, 234, 235, 236, 238, 239, 239, 240,

Issue array : 200, 201, 202, 203, 204, 155, 206, 207, 208, 209, 210, 211, 212, 213, 264, 265, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 183, 234, 235, 236, 287, 238, 239, 240,
Moving average  : 200, 201, 202, 196, 197, 198, 199, 200, 201, 209, 210, 218, 226, 227, 228, 229, 230, 231, 225, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 223, 224, 225, 226, 234, 235, 236, 244, 248, 239, 240,

It can be seen that the moving median better balances the intensive random data outliers.

For the real data:

Array x80c48 processing:

Issue array : 10790, 10728, 10565, 10228, 10148, 9911, 9861, 9880, 9887, 9894, 9907, 9910, 9917, 9932, 9937, 9925, 9900, 9684, 9579, 9446, 9040, 8912, 8703, 8457, 8350, 8338, 8129, 8040, 7900, 7836, 7731, 7490, 7271, 7250, 7165, 7013, 6912, 6848, 6823, 6857, 6868, 6894, 6902, 6903, 6904, 7114, 7067, 7047, 6994, 6883, 6848, 6787, 6722, 6412, 6236, 6136, 6012, 5991, 5659, 5648, 5595, 5327, 5079, 5015, 4678, 4639, 4569, 4294, 4241, 4164, 4137, 3948, 3905, 3771, 3731, 3664, 3577, 3422, 3304, 3086, 3053, 2977, 2967, 3248, 3257, 3202, 2954, 2834, 2594, 2574, 2611, 2730, 2766, 2514, 2387, 2368, 2365, 2344, 2312, 2098, 1905, 1722, 1579, 1233, 1206, 963, 815, 825,
Slide medians  : 10790, 10728, 10565, 10228, 10148, 9911, 9894, 9894, 9894, 9894, 9907, 9910, 9917, 9917, 9917, 9917, 9900, 9684, 9579, 9446, 9040, 8912, 8703, 8457, 8350, 8338, 8129, 8040, 7900, 7836, 7731, 7490, 7271, 7250, 7165, 7013, 6912, 6868, 6868, 6868, 6868, 6894, 6902, 6903, 6904, 6994, 6994, 6994, 6994, 6883, 6848, 6787, 6722, 6412, 6236, 6136, 6012, 5991, 5659, 5648, 5595, 5327, 5079, 5015, 4678, 4639, 4569, 4294, 4241, 4164, 4137, 3948, 3905, 3771, 3731, 3664, 3577, 3422, 3304, 3086, 3086, 3086, 3086, 3053, 2977, 2967, 2954, 2834, 2730, 2730, 2611, 2594, 2574, 2514, 2387, 2368, 2365, 2344, 2312, 2098, 1905, 1722, 1579, 1233, 1206, 963, 825, 825,

Issue array : 10790, 10728, 10565, 10228, 10148, 9911, 9861, 9880, 9887, 9894, 9907, 9910, 9917, 9932, 9937, 9925, 9900, 9684, 9579, 9446, 9040, 8912, 8703, 8457, 8350, 8338, 8129, 8040, 7900, 7836, 7731, 7490, 7271, 7250, 7165, 7013, 6912, 6848, 6823, 6857, 6868, 6894, 6902, 6903, 6904, 7114, 7067, 7047, 6994, 6883, 6848, 6787, 6722, 6412, 6236, 6136, 6012, 5991, 5659, 5648, 5595, 5327, 5079, 5015, 4678, 4639, 4569, 4294, 4241, 4164, 4137, 3948, 3905, 3771, 3731, 3664, 3577, 3422, 3304, 3086, 3053, 2977, 2967, 3248, 3257, 3202, 2954, 2834, 2594, 2574, 2611, 2730, 2766, 2514, 2387, 2368, 2365, 2344, 2312, 2098, 1905, 1722, 1579, 1233, 1206, 963, 815, 825,
Moving average  : 10790, 10694, 10492, 10319, 10189, 10069, 9973, 9927, 9893, 9894, 9904, 9912, 9917, 9918, 9886, 9839, 9772, 9644, 9498, 9323, 9117, 8927, 8749, 8561, 8418, 8274, 8150, 8046, 7923, 7771, 7645, 7520, 7394, 7262, 7136, 7040, 6981, 6927, 6888, 6872, 6871, 6879, 6920, 6950, 6976, 6990, 6987, 6980, 6963, 6907, 6813, 6697, 6575, 6450, 6328, 6167, 6013, 5897, 5767, 5616, 5473, 5286, 5140, 4986, 4800, 4645, 4514, 4389, 4285, 4180, 4066, 3985, 3903, 3819, 3717, 3625, 3508, 3405, 3298, 3198, 3151, 3127, 3113, 3094, 3063, 3008, 2952, 2861, 2786, 2723, 2660, 2597, 2564, 2534, 2496, 2437, 2341, 2254, 2159, 2046, 1885, 1722, 1529, 1346, 1192, 1008, 868, 825,

Array y80c48 processing:

Issue array : 7289, 7275, 7243, 7178, 7163, 7119, 7109, 6903, 6785, 6680, 6448, 6379, 6264, 5869, 5709, 5448, 5353, 5083, 5081, 5083, 5063, 5062, 5042, 5166, 5186, 5172, 4916, 4925, 4982, 5009, 5025, 5007, 4993, 4991, 4986, 4976, 4968, 4964, 4962, 4636, 4488, 4148, 4038, 4029, 4010, 3960, 3798, 3709, 3462, 3475, 3480, 3489, 3498, 3542, 3567, 3581, 3598, 3586, 3647, 3649, 3656, 3693, 3727, 3736, 3782, 3788, 3797, 3831, 3837, 3743, 3710, 3550, 3511, 3406, 3379, 3435, 3520, 3455, 3367, 3204, 3179, 3123, 3115, 2534, 2519, 2472, 2351, 2281, 2141, 2129, 2066, 1862, 1801, 1522, 1302, 1218, 1200, 1038, 851, 812, 778, 746, 721, 621, 529, 420, 386, 279,
Slide medians  : 7289, 7275, 7243, 7178, 7163, 7119, 7109, 6903, 6785, 6680, 6448, 6379, 6264, 5869, 5709, 5448, 5353, 5083, 5083, 5081, 5081, 5081, 5083, 5063, 5062, 5042, 5009, 5009, 5007, 4993, 4993, 4993, 4993, 4991, 4986, 4976, 4968, 4964, 4962, 4636, 4488, 4148, 4038, 4029, 4010, 3960, 3798, 3709, 3489, 3489, 3489, 3489, 3498, 3542, 3567, 3581, 3586, 3598, 3647, 3649, 3656, 3693, 3727, 3736, 3782, 3788, 3788, 3788, 3788, 3743, 3710, 3550, 3511, 3511, 3455, 3435, 3406, 3379, 3367, 3204, 3179, 3123, 3115, 2534, 2519, 2472, 2351, 2281, 2141, 2129, 2066, 1862, 1801, 1522, 1302, 1218, 1200, 1038, 851, 812, 778, 746, 721, 621, 529, 420, 386, 279,

Issue array : 7289, 7275, 7243, 7178, 7163, 7119, 7109, 6903, 6785, 6680, 6448, 6379, 6264, 5869, 5709, 5448, 5353, 5083, 5081, 5083, 5063, 5062, 5042, 5166, 5186, 5172, 4916, 4925, 4982, 5009, 5025, 5007, 4993, 4991, 4986, 4976, 4968, 4964, 4962, 4636, 4488, 4148, 4038, 4029, 4010, 3960, 3798, 3709, 3462, 3475, 3480, 3489, 3498, 3542, 3567, 3581, 3598, 3586, 3647, 3649, 3656, 3693, 3727, 3736, 3782, 3788, 3797, 3831, 3837, 3743, 3710, 3550, 3511, 3406, 3379, 3435, 3520, 3455, 3367, 3204, 3179, 3123, 3115, 2534, 2519, 2472, 2351, 2281, 2141, 2129, 2066, 1862, 1801, 1522, 1302, 1218, 1200, 1038, 851, 812, 778, 746, 721, 621, 529, 420, 386, 279,
Moving average  : 7289, 7269, 7230, 7197, 7141, 7071, 6991, 6887, 6775, 6653, 6475, 6305, 6114, 5924, 5729, 5544, 5375, 5260, 5168, 5110, 5083, 5098, 5111, 5087, 5067, 5056, 5051, 5031, 5005, 4980, 4990, 4999, 4998, 4992, 4984, 4977, 4926, 4854, 4735, 4601, 4466, 4330, 4187, 4067, 3956, 3858, 3778, 3699, 3625, 3559, 3522, 3502, 3519, 3536, 3552, 3574, 3596, 3612, 3630, 3651, 3671, 3699, 3719, 3740, 3765, 3785, 3788, 3784, 3751, 3711, 3655, 3591, 3533, 3502, 3465, 3439, 3395, 3363, 3326, 3280, 3140, 3006, 2878, 2756, 2628, 2488, 2347, 2280, 2186, 2090, 1972, 1832, 1700, 1567, 1420, 1276, 1135, 1028, 949, 878, 795, 723, 661, 600, 529, 447, 362, 279,


Compared with the moving average algorithm, the slide medians treats data much more carefully.