misc backup
This commit is contained in:
parent
9f36baa83c
commit
9ca0389b30
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue