diff --git a/calibration_plot.m b/calibration_plot.m index 7b75a0c..c5acbdc 100644 --- a/calibration_plot.m +++ b/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)); diff --git a/calibration_walking.m b/calibration_walking.m index caba669..898e47a 100644 --- a/calibration_walking.m +++ b/calibration_walking.m @@ -67,4 +67,4 @@ function diffs = calibration_walking(filename) % cdfplot(diffs); % % histogram(diffs, 'BinWidth', 1, 'Normalization', 'pdf'); -end \ No newline at end of file +end diff --git a/config_entry.default b/config_entry.default index 06002bd..654ea9d 100755 --- a/config_entry.default +++ b/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" } } diff --git a/distribution_range.m b/distribution_range.m new file mode 100644 index 0000000..c2dc68b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/libLocalization.py b/libLocalization.py index 15b8c06..324a753 100755 --- a/libLocalization.py +++ b/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( - { - 'config_entry': { - '34:f6:4b:5e:69:1f': {'location': '0,0'}, - '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, - 'outfp': '', - 'loc_bounds': { - 'y_min': 0 - } - }, - { - '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 = True - except Exception: - pass - if flagPlot: - fig = plt.figure() - plt.ion() - plt.xlim([-100, 300]) - plt.ylim([-10, 500]) - while 1: - try: - handler = plotLocation(loc) - if handler is None: - plt.close(fig) - break - handler.remove() - except KeyboardInterrupt: - plt.close(fig) - break - except Exception: - raise + dist = get_distance_gps([41.790366, -87.601111], [41.790609, -87.601411]) + print(dist) + # loc = deriveLocation( + # { + # 'config_entry': { + # '34:f6:4b:5e:69:1f': {'location': '0,0'}, + # '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, + # 'outfp': '', + # 'loc_bounds': { + # 'y_min': 0 + # } + # }, + # { + # '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 = True + # except Exception: + # pass + # if flagPlot: + # fig = plt.figure() + # plt.ion() + # plt.xlim([-100, 300]) + # plt.ylim([-10, 500]) + # while 1: + # try: + # handler = plotLocation(loc) + # if handler is None: + # plt.close(fig) + # break + # handler.remove() + # except KeyboardInterrupt: + # plt.close(fig) + # break + # except Exception: + # raise diff --git a/localization_static.py b/localization_static.py new file mode 100755 index 0000000..03bd933 --- /dev/null +++ b/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()