misc backup

calibration_plot.m

@@ -1,5 +1,5 @@
 % clear all; %close all;
-
+param_linear = [0.8927, 553.3157];
 folder = 'calibration_data/outdoor/';
 files = dir(folder); files = files(3:end);  % remove . and ..
 targets = zeros(1, length(files));

config_entry.default

@@ -4,13 +4,13 @@
         "cf": 2462,
         "spb": 255,
         "retries": 3,
-        "location": "0,0"
+        "location": "500,0"
     },
     "34:f6:4b:5e:69:0b": {
         "bw": 20,
         "cf": 2437,
         "spb": 255,
         "retries": 3,
-        "location": "180,0"
+        "location": "0,0"
     }
 }

distribution_range.m

@@ -0,0 +1,167 @@
+files0b = {}; files1f = {};
+targets0b = []; targets1f = [];
+
+% dataset A
+folder = 'calibration_data/02222018_outdoor_500/';
+files = dir(folder); files = files(3:end);  % remove . and ..
+for i = length(files):-1:1
+    if (~contains(files(i).name, 'result') ||...
+            ~contains(files(i).name, 'extract') ||...
+            contains(files(i).name, 'locs'))
+        
+    elseif contains(files(i).name, '34-f6-4b-5e-69-1f')
+        files1f = [files1f, [folder, files(i).name]];
+        loc = sscanf(files(i).name, 'result_static_%f_%f_extract.txt');
+        targets1f = [targets1f, sqrt(sum((loc - [5;0]).^2)) * 100];
+    else
+        files0b = [files0b, [folder, files(i).name]];
+        loc = sscanf(files(i).name, 'result_static_%f_%f_extract.txt');
+        targets0b = [targets0b, sqrt(sum((loc - [0;0]).^2)) * 100];
+    end
+end
+
+% dataset B
+folder = 'calibration_data/02142018_outdoor_480/';
+files = dir(folder); files = files(3:end);  % remove . and ..
+for i = length(files):-1:1
+    if (~contains(files(i).name, 'result') ||...
+            ~contains(files(i).name, 'extract') ||...
+            contains(files(i).name, 'locs'))
+        
+    elseif contains(files(i).name, '34-f6-4b-5e-69-1f')
+        files1f = [files1f, [folder, files(i).name]];
+        loc = sscanf(files(i).name, 'result_static_%f_%f_extract.txt');
+        targets1f = [targets1f, sqrt(sum((loc - [4.8;0]).^2)) * 100];
+    else
+        files0b = [files0b, [folder, files(i).name]];
+        loc = sscanf(files(i).name, 'result_static_%f_%f_extract.txt');
+        targets0b = [targets0b, sqrt(sum((loc - [0;0]).^2)) * 100];
+    end
+end
+
+% % dataset C
+% folder = 'calibration_data/outdoor/';
+% files = dir(folder); files = files(3:end);  % remove . and ..
+% for i = length(files):-1:1
+%     if (~contains(files(i).name, 'result'))
+%         
+%     else
+%         files1f = [files1f, [folder, files(i).name]];
+%         targets1f = [targets1f, sscanf(files(i).name, 'result_%dcm.txt')];
+%     end
+% end
+
+[targets0b, orderI] = sort(targets0b);
+files0b = files0b(orderI);
+[targets1f, orderI] = sort(targets1f);
+files1f = files1f(orderI);
+
+all_data0b = [];
+all_data1f = [];
+% figure(1); clf; 
+for i = 1:length(files0b)
+    filename = files0b{i};
+    
+    fileID = fopen(filename, 'r');
+    formatSpec = [...
+        'Target: %x:%x:%x:%x:%x:%x, status: %d, ',...
+        'rtt: %d psec, distance: %d cm\n'...
+    ];
+    data = fscanf(fileID, formatSpec, [9 Inf]);
+    fclose(fileID);
+    if isempty(data)
+        data = readtable(filename, 'ReadVariableNames', 0);
+        if isempty(data)
+            continue
+        end
+        data = data(2:end, :);
+        caliDist = str2double(table2array(data(:, 2)))';
+        rawRTT = str2double(table2array(data(:, 3)))';
+        rawRTTStd = sqrt(str2double(table2array(data(:, 4)))');
+        rawDist = str2double(table2array(data(:, 5)))';
+        rawDistStd = sqrt(str2double(table2array(data(:, 6)))');
+        rssi = str2double(table2array(data(:, 7)))';
+        time = str2double(table2array(data(:, 8)))';
+        logistics = isnan(caliDist) | isnan(time);
+    else
+        % get rid of invalid data
+        data(:, data(7, :) ~= 0) = [];
+        data(:, data(9, :) < -1000) = [];
+        rawDist = data(9, :);
+        rawDistStd = zeros(size(rawDist));
+        rssi = zeros(size(rawDist));
+        caliDist = 0.8927 * rawDist + 553.3157;
+        logistics = isnan(caliDist);
+    end
+    caliDist(logistics) = [];
+    rssi(logistics) = [];
+%     figure(1); cdfplot(caliDist);
+    
+    err = nanmedian(caliDist) - targets0b(i);
+    fprintf(...
+        '%.2f: rssi %.2f sig std %.2f dist std %.2f err %.2f\n',...
+        targets0b(i), nanmedian(rssi), nanstd(rssi), nanstd(caliDist), err)
+%     all_data0b = [all_data0b;...
+%         [nanmedian(rssi), nanstd(rssi),...
+%         nanmedian(rawDistStd), nanstd(rawDistStd), err]];
+    all_data0b = [all_data0b, caliDist - targets0b(i)];
+end
+for i = 1:length(files1f)
+    filename = files1f{i};
+    
+    fileID = fopen(filename, 'r');
+    formatSpec = [...
+        'Target: %x:%x:%x:%x:%x:%x, status: %d, ',...
+        'rtt: %d psec, distance: %d cm\n'...
+    ];
+    data = fscanf(fileID, formatSpec, [9 Inf]);
+    fclose(fileID);
+    if isempty(data)
+        data = readtable(filename, 'ReadVariableNames', 0);
+        if isempty(data)
+            continue
+        end
+        data = data(2:end, :);
+        caliDist = str2double(table2array(data(:, 2)))';
+        rawRTT = str2double(table2array(data(:, 3)))';
+        rawRTTStd = sqrt(str2double(table2array(data(:, 4)))');
+        rawDist = str2double(table2array(data(:, 5)))';
+        rawDistStd = sqrt(str2double(table2array(data(:, 6)))');
+        rssi = str2double(table2array(data(:, 7)))';
+        time = str2double(table2array(data(:, 8)))';
+        logistics = isnan(caliDist) | isnan(time);
+    else
+        % get rid of invalid data
+        data(:, data(7, :) ~= 0) = [];
+        data(:, data(9, :) < -1000) = [];
+        rawDist = data(9, :);
+        rawDistStd = zeros(size(rawDist));
+        rssi = zeros(size(rawDist));
+        caliDist = 0.8927 * rawDist + 553.3157;
+        logistics = isnan(caliDist);
+    end
+    caliDist(logistics) = [];
+    rssi(logistics) = [];
+%     figure(1); cdfplot(caliDist - targets1f(i));
+    
+    err = nanmedian(caliDist) - targets1f(i);
+    fprintf(...
+        '%.2f: rssi %.2f sig std %.2f dist std %.2f err %.2f\n',...
+        targets1f(i), nanmedian(rssi), nanstd(rssi), nanstd(caliDist), err)
+%     all_data1f = [all_data1f;...
+%         [nanmedian(rssi), nanstd(rssi),...
+%         nanmedian(rawDistStd), nanstd(rawDistStd), err]];
+    all_data1f = [all_data1f, caliDist - targets1f(i)];
+end
+
+figure(2); clf; hold on;
+cdfplot(all_data0b); cdfplot(all_data1f);
+hold off; legend('0b', '1f');
+pd1f = fitdist(all_data1f', 'Normal');
+pd0b = fitdist(all_data0b', 'Normal');
+pd = fitdist([all_data0b, all_data1f]', 'Normal');
+
+%  Normal distribution
+%        mu = -5.77619   [-7.52889, -4.02349] <- measurement error
+%     sigma =  52.8182   [51.6078, 54.0872]
+% The sigma is consistent across observations in multiple ranging locations

libLocalization.py

@@ -6,8 +6,11 @@ import time
 import math
 import argparse
 
+from math import sin, cos, sqrt, atan2, radians
 from numpy import median, arange
 
+R = 6373000.0  # unit: meter
+
 # ======= start =======
 # excerpt from
 # https://github.com/noomrevlis/trilateration/blob/master/trilateration2D.py
@@ -29,12 +32,15 @@ class Circle(object):
 
 
 def get_distance(p1, p2):
-    if isinstance(p1, Point):
+    if isinstance(p1, Point) and isinstance(p2, Point):
         return math.sqrt(
             (p1.x - p2.x) * (p1.x - p2.x) +
             (p1.y - p2.y) * (p1.y - p2.y)
         )
-    elif isinstance(p1, list) or isinstance(p1, tuple):
+    elif (
+        (isinstance(p1, list) or isinstance(p1, tuple)) and
+        (isinstance(p2, list) or isinstance(p2, tuple))
+    ):
         return math.sqrt(
             (p1[0] - p2[0]) * (p1[0] - p2[0]) +
             (p1[1] - p2[1]) * (p1[1] - p2[1])
@@ -42,6 +48,30 @@ def get_distance(p1, p2):
     return -1
 
 
+def get_distance_gps(p1, p2, isDeg=True):
+    # format: p1 ~ p2 = [lat, lon] in deg
+    if (
+        (isinstance(p1, list) or isinstance(p1, tuple)) and
+        (isinstance(p2, list) or isinstance(p2, tuple))
+    ):
+        if isDeg:
+            lat1 = radians(p1[0])
+            lon1 = radians(p1[1])
+            lat2 = radians(p2[0])
+            lon2 = radians(p2[1])
+        else:
+            lat1 = (p1[0])
+            lon1 = (p1[1])
+            lat2 = (p2[0])
+            lon2 = (p2[1])
+        dlon = lon2 - lon1
+        dlat = lat2 - lat1
+        a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
+        c = 2 * atan2(sqrt(a), sqrt(1 - a))
+        return R * c
+    return -1
+
+
 def get_two_circles_intersecting_points(c1, c2):
     p1 = c1.center
     p2 = c2.center
@@ -190,46 +220,49 @@ def plotLocation(loc):
         pass
     return handler
 
+
 if __name__ == '__main__':
-    loc = deriveLocation(
+    dist = get_distance_gps([41.790366, -87.601111], [41.790609, -87.601411])
-        {
+    print(dist)
-            'config_entry': {
+    # loc = deriveLocation(
-                '34:f6:4b:5e:69:1f': {'location': '0,0'},
+    #     {
-                '34:f6:4b:5e:69:1e': {'location': '180,0'},
+    #         'config_entry': {
-                '34:f6:4b:5e:69:1d': {'location': '0,2'},
+    #             '34:f6:4b:5e:69:1f': {'location': '0,0'},
-                '34:f6:4b:5e:69:1a': {'location': '1,2'}
+    #             '34:f6:4b:5e:69:1e': {'location': '180,0'},
-            },
+    #             '34:f6:4b:5e:69:1d': {'location': '0,2'},
-            'verbose': True,
+    #             '34:f6:4b:5e:69:1a': {'location': '1,2'}
-            'outfp': '',
+    #         },
-            'loc_bounds': {
+    #         'verbose': True,
-                'y_min': 0
+    #         'outfp': '',
-            }
+    #         'loc_bounds': {
-        },
+    #             'y_min': 0
-        {
+    #         }
-            '34:f6:4b:5e:69:1f': (257, 0),
+    #     },
-            '34:f6:4b:5e:69:1e': (50, 50)
+    #     {
-        }
+    #         '34:f6:4b:5e:69:1f': (257, 0),
-    )
+    #         '34:f6:4b:5e:69:1e': (50, 50)
-    flagPlot = False
+    #     }
-    try:
+    # )
-        import matplotlib.pyplot as plt
+    # flagPlot = False
-        flagPlot = True
+    # try:
-    except Exception:
+    #     import matplotlib.pyplot as plt
-        pass
+    #     flagPlot = True
-    if flagPlot:
+    # except Exception:
-        fig = plt.figure()
+    #     pass
-        plt.ion()
+    # if flagPlot:
-        plt.xlim([-100, 300])
+    #     fig = plt.figure()
-        plt.ylim([-10, 500])
+    #     plt.ion()
-        while 1:
+    #     plt.xlim([-100, 300])
-            try:
+    #     plt.ylim([-10, 500])
-                handler = plotLocation(loc)
+    #     while 1:
-                if handler is None:
+    #         try:
-                    plt.close(fig)
+    #             handler = plotLocation(loc)
-                    break
+    #             if handler is None:
-                handler.remove()
+    #                 plt.close(fig)
-            except KeyboardInterrupt:
+    #                 break
-                plt.close(fig)
+    #             handler.remove()
-                break
+    #         except KeyboardInterrupt:
-            except Exception:
+    #             plt.close(fig)
-                raise
+    #             break
+    #         except Exception:
+    #             raise

localization_static.py

@@ -0,0 +1,128 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+
+import re
+import os
+import time
+import json
+import argparse
+from numpy import nanstd, nanmedian, sqrt
+from libExtraction import separateMAC
+from libLocalization import deriveLocation, get_distance
+
+
+def get_known_locs(results, args):
+    database = json.load(open(args['json'], 'r'))
+    for mac in database.keys():
+        if mac not in results:
+            del database[mac]
+    for mac in results.keys():
+        if mac not in database:
+            del results[mac]
+    return database
+
+
+def wrapper(args):
+    if not os.path.isfile(args['filepath']):
+        return
+    results, nameLine, startLine, endLine = separateMAC(args['filepath'])
+    args['config_entry'] = get_known_locs(results, args)
+    # method 1: average and then compute single loc
+    final_result = {}
+    for mac in results:
+        dists = []
+        for line in results[mac]:
+            try:
+                # tmp = float(line.split(',')[4])
+                # ugly hack
+                # if mac == '34:f6:4b:5e:69:1f':
+                #     tmp = tmp * 0.7927 + 483.3157  # med school fit for 1f
+                # elif mac == '34:f6:4b:5e:69:0b':
+                #     tmp = tmp * 0.6927 + 400.3157  # med school fit for 0b
+                tmp = float(line.split(',')[1])  # already fitted result
+                dists.append(tmp)
+            except Exception as e:
+                print(e)
+        final_result[mac] = (nanmedian(dists), nanstd(dists))
+    print('est:', final_result)
+    loc = deriveLocation(args, final_result)
+    print('loc:', loc)
+    # # method 2: do localization first for every pair and then average
+    # all_locs = {}
+    # if results:
+    #     keys = results.keys()
+    #     idxs = [0] * len(keys)
+    #     locs = []
+    #     while all([idxs[i] < len(results[keys[i]]) for i in range(len(idxs))]):
+    #         lines = [results[keys[i]][idxs[i]] for i in range(len(idxs))]
+    #         times = [float(x.split(',')[7]) for x in lines]
+    #         maxT = max(times)
+    #         if all([abs(t - maxT) < 0.01 for t in times]):
+    #             dists = [
+    #                 (float(x.split(',')[1]), sqrt(float(x.split(',')[5])))
+    #                 for x in lines
+    #             ]
+    #             loc = deriveLocation(args, dict(zip(keys, dists)))
+    #             print('{0:.4f},{1:.4f}'.format(loc[0], loc[1]))
+    #             locs.append(loc)
+    #             for i in range(len(idxs)):
+    #                 idxs[i] += 1
+    #         else:
+    #             for i in range(len(idxs)):
+    #                 if abs(times[i] - maxT) > 0.01:
+    #                     idxs[i] += 1
+    #     x, y = zip(*locs)
+    #     loc = (nanmedian(x), nanmedian(y))
+    #     locstd = (nanstd(x), nanstd(y))
+    #     print(loc)
+    #     print(locstd)
+    match = re.search(r"static_([0-9.]+)_([0-9.]+)_", args['filepath'])
+    if match:
+        trueX = float(match.group(1)) * 100
+        trueY = float(match.group(2)) * 100
+        true_result = {}
+        for mac in args['config_entry']:
+            mac_loc = args['config_entry'][mac]['location'].split(',')
+            mac_loc = (float(mac_loc[0]), float(mac_loc[1]))
+            true_result[mac] = (get_distance(mac_loc, (trueX, trueY)), 0)
+        print('true:', true_result)
+        err = get_distance(loc, (trueX, trueY))
+        print('err:', err)
+
+
+def main():
+    p = argparse.ArgumentParser(description='separate data from MAC addr')
+    p.add_argument(
+        'filepath',
+        help="file path"
+    )
+    p.add_argument(
+        '--outfp', '-f',
+        default=None,
+        help="if set, will write raw fetched data to file"
+    )
+    p.add_argument(
+        '--json', '-j',
+        default='config_entry.default',
+        help="load a config json file"
+    )
+    p.add_argument(
+        '--verbose', '-v',
+        default=False,
+        action="store_true",
+        help="if set, show detailed messages"
+    )
+    try:
+        args = vars(p.parse_args())
+    except Exception as e:
+        print(str(e))
+        sys.exit()
+    args['time_of_exec'] = int(time.time())
+    # TODO: manually add bounds
+    args['loc_bounds'] = {'y_min': 0}
+    wrapper(args)
+
+
+if __name__ == '__main__':
+    main()