From 31828a608810b3909088cac543a697dedd537c00 Mon Sep 17 00:00:00 2001
From: HappyZ <zhuyanzi@gmail.com>
Date: Thu, 8 Feb 2018 14:03:26 -0600
Subject: [PATCH] improve localization trilateration

---
 .gitignore           |  1 +
 config_entry.default | 10 +++++++++-
 libLocalization.py   | 41 +++++++++++++++++++++++++----------------
 libMeasurement.py    | 15 +++++++++++++--
 4 files changed, 48 insertions(+), 19 deletions(-)

diff --git a/.gitignore b/.gitignore
index 2debb76..e1fa86f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,3 +6,4 @@ iw/.config
 iw/version.c
 iw/iw.8.gz
 iw/*-stamp
+*.pyc
diff --git a/config_entry.default b/config_entry.default
index 02e8501..06002bd 100755
--- a/config_entry.default
+++ b/config_entry.default
@@ -3,6 +3,14 @@
         "bw": 20,
         "cf": 2462,
         "spb": 255,
-        "retries": 3
+        "retries": 3,
+        "location": "0,0"
+    },
+    "34:f6:4b:5e:69:0b": {
+        "bw": 20,
+        "cf": 2437,
+        "spb": 255,
+        "retries": 3,
+        "location": "180,0"
     }
 }
diff --git a/libLocalization.py b/libLocalization.py
index 90afabe..d0a7824 100755
--- a/libLocalization.py
+++ b/libLocalization.py
@@ -6,7 +6,7 @@ import time
 import math
 import argparse
 
-from numpy import median
+from numpy import median, arange
 
 # ======= start =======
 # excerpt from
@@ -97,11 +97,12 @@ def get_polygon_center(points):
 
 def trilateration2d(mydict, bounds=None, verbose=False):
     '''
+    mydict format: {
+        location: (radius, std),...
+    }
     bound format: {
-        'x_min': float,
-        'x_max': float,
-        'y_min': float,
-        'y_max': float
+        'x_min': float, 'x_max': float,
+        'y_min': float, 'y_max': float
     }
     '''
     points = []
@@ -110,13 +111,21 @@ def trilateration2d(mydict, bounds=None, verbose=False):
         tmp = loc.split(',')
         p = Point(tmp[0], tmp[1])
         points.append(p)
-        c = Circle(p, mydict[loc])
-        circles.append(c)
+        if mydict[loc][1]:
+            # create multiple circles based on std
+            for r in arange(
+                mydict[loc][0] - mydict[loc][1],
+                mydict[loc][0] + mydict[loc][1],
+                10
+            ):
+                circles.append(Circle(p, r))
+        else:
+            circles.append(Circle(p, mydict[loc]))
     # print(len(points), len(circles))
     inner_points = []
     for p in get_intersecting_points(circles):
-        if not is_contained_in_circles(p, circles):
-            continue
+        # if not is_contained_in_circles(p, circles):
+        #     continue
         if bounds is not None:
             if bounds.get('x_min', None) is not None and bounds['x_min'] > p.x:
                 continue
@@ -162,23 +171,23 @@ def deriveLocation(args, results):
 
 
 if __name__ == '__main__':
-    deriveLocation(
+    loc = deriveLocation(
         {
             'config_entry': {
                 '34:f6:4b:5e:69:1f': {'location': '0,0'},
-                '34:f6:4b:5e:69:1e': {'location': '1,1'},
+                '34:f6:4b:5e:69:1e': {'location': '180,0'},
                 '34:f6:4b:5e:69:1d': {'location': '0,2'},
                 '34:f6:4b:5e:69:1a': {'location': '1,2'}
             },
             'verbose': True,
-            'filepath': 'test.txt',
+            'filepath': '',
             'loc_bounds': {
-                'x_min': 0
+                'y_min': 0
             }
         },
         {
-            '34:f6:4b:5e:69:1f': 100,
-            '34:f6:4b:5e:69:1e': 100,
-            '34:f6:4b:5e:69:1d': 220
+            '34:f6:4b:5e:69:1f': (257, 50),
+            '34:f6:4b:5e:69:1e': (50, 50)
         }
     )
+    print(loc)
diff --git a/libMeasurement.py b/libMeasurement.py
index 277cfb0..c11caaa 100755
--- a/libMeasurement.py
+++ b/libMeasurement.py
@@ -162,7 +162,13 @@ class Measurement(object):
                     result[each[0]] = []
                 result[each[0]].append(each[1:])
         for mac in result:
-            median_result[mac] = median([x[0] for x in result[mac]])
+            median_result[mac] = (
+                median([x[0] for x in result[mac]]),
+                median(
+                    [sqrt(x[4]) * self.cali[0] + self.cali[1]
+                     for x in result[mac]]
+                )
+            )
         return median_result
 
     def __enter__(self):
@@ -205,7 +211,10 @@ def wrapper(args):
                     rounds=args['rounds'], verbose=args['verbose']
                 )
                 for mac in results:
-                    print('* {0} is {1:.4f}cm away.'.format(mac, results[mac]))
+                    print(
+                        '* {0} is {1:.4f}cm (±{2:.2f}) away.'
+                        .format(mac, results[mac][0], results[mac][1])
+                    )
                 # calculate location info
                 if args['locs']:
                     loc = deriveLocation(args, results)
@@ -284,6 +293,8 @@ def main():
     if args['indoor'] and args['cali'] == (0.8927, 553.3157):
         args['cali'] = (0.9376, 558.0551)
     args['time_of_exec'] = int(time.time())
+    # TODO: add option to change loc bounds, currently force y_min = 0
+    args['loc_bounds'] = {'y_min': 0}
     # rename file path by adding time of exec
     if args['filepath']:
         fp, ext = os.path.splitext(args['filepath'])