Compare commits
No commits in common. "develop" and "master" have entirely different histories.
|
|
@ -1,9 +1,7 @@
|
|||
calibration_data/
|
||||
iw/iw
|
||||
iw/*~
|
||||
iw/*.o
|
||||
iw/.config
|
||||
iw/version.c
|
||||
iw/iw.8.gz
|
||||
iw/*-stamp
|
||||
*.pyc
|
||||
iw
|
||||
*~
|
||||
*.o
|
||||
.config
|
||||
version.c
|
||||
iw.8.gz
|
||||
*-stamp
|
||||
|
|
|
|||
453
allfitdist.m
453
allfitdist.m
|
|
@ -1,453 +0,0 @@
|
|||
function [D PD] = allfitdist(data,sortby,varargin)
|
||||
%ALLFITDIST Fit all valid parametric probability distributions to data.
|
||||
% [D PD] = ALLFITDIST(DATA) fits all valid parametric probability
|
||||
% distributions to the data in vector DATA, and returns a struct D of
|
||||
% fitted distributions and parameters and a struct of objects PD
|
||||
% representing the fitted distributions. PD is an object in a class
|
||||
% derived from the ProbDist class.
|
||||
%
|
||||
% [...] = ALLFITDIST(DATA,SORTBY) returns the struct of valid distributions
|
||||
% sorted by the parameter SORTBY
|
||||
% NLogL - Negative of the log likelihood
|
||||
% BIC - Bayesian information criterion (default)
|
||||
% AIC - Akaike information criterion
|
||||
% AICc - AIC with a correction for finite sample sizes
|
||||
%
|
||||
% [...] = ALLFITDIST(...,'DISCRETE') specifies it is a discrete
|
||||
% distribution and does not attempt to fit a continuous distribution
|
||||
% to the data
|
||||
%
|
||||
% [...] = ALLFITDIST(...,'PDF') or (...,'CDF') plots either the PDF or CDF
|
||||
% of a subset of the fitted distribution. The distributions are plotted in
|
||||
% order of fit, according to SORTBY.
|
||||
%
|
||||
% List of distributions it will try to fit
|
||||
% Continuous (default)
|
||||
% Beta
|
||||
% Birnbaum-Saunders
|
||||
% Exponential
|
||||
% Extreme value
|
||||
% Gamma
|
||||
% Generalized extreme value
|
||||
% Generalized Pareto
|
||||
% Inverse Gaussian
|
||||
% Logistic
|
||||
% Log-logistic
|
||||
% Lognormal
|
||||
% Nakagami
|
||||
% Normal
|
||||
% Rayleigh
|
||||
% Rician
|
||||
% t location-scale
|
||||
% Weibull
|
||||
%
|
||||
% Discrete ('DISCRETE')
|
||||
% Binomial
|
||||
% Negative binomial
|
||||
% Poisson
|
||||
%
|
||||
% Optional inputs:
|
||||
% [...] = ALLFITDIST(...,'n',N,...)
|
||||
% For the 'binomial' distribution only:
|
||||
% 'n' A positive integer specifying the N parameter (number
|
||||
% of trials). Not allowed for other distributions. If
|
||||
% 'n' is not given it is estimate by Method of Moments.
|
||||
% If the estimated 'n' is negative then the maximum
|
||||
% value of data will be used as the estimated value.
|
||||
% [...] = ALLFITDIST(...,'theta',THETA,...)
|
||||
% For the 'generalized pareto' distribution only:
|
||||
% 'theta' The value of the THETA (threshold) parameter for
|
||||
% the generalized Pareto distribution. Not allowed for
|
||||
% other distributions. If 'theta' is not given it is
|
||||
% estimated by the minimum value of the data.
|
||||
%
|
||||
% Note: ALLFITDIST does not handle nonparametric kernel-smoothing,
|
||||
% use FITDIST directly instead.
|
||||
%
|
||||
%
|
||||
% EXAMPLE 1
|
||||
% Given random data from an unknown continuous distribution, find the
|
||||
% best distribution which fits that data, and plot the PDFs to compare
|
||||
% graphically.
|
||||
% data = normrnd(5,3,1e4,1); %Assumed from unknown distribution
|
||||
% [D PD] = allfitdist(data,'PDF'); %Compute and plot results
|
||||
% D(1) %Show output from best fit
|
||||
%
|
||||
% EXAMPLE 2
|
||||
% Given random data from a discrete unknown distribution, with frequency
|
||||
% data, find the best discrete distribution which would fit that data,
|
||||
% sorted by 'NLogL', and plot the PDFs to compare graphically.
|
||||
% data = nbinrnd(20,.3,1e4,1);
|
||||
% values=unique(data); freq=histc(data,values);
|
||||
% [D PD] = allfitdist(values,'NLogL','frequency',freq,'PDF','DISCRETE');
|
||||
% PD{1}
|
||||
%
|
||||
% EXAMPLE 3
|
||||
% Although the Geometric Distribution is not listed, it is a special
|
||||
% case of fitting the more general Negative Binomial Distribution. The
|
||||
% parameter 'r' should be close to 1. Show by example.
|
||||
% data=geornd(.7,1e4,1); %Random from Geometric
|
||||
% [D PD]= allfitdist(data,'PDF','DISCRETE');
|
||||
% PD{1}
|
||||
%
|
||||
% EXAMPLE 4
|
||||
% Compare the resulting distributions under two different assumptions
|
||||
% of discrete data. The first, that it is known to be derived from a
|
||||
% Binomial Distribution with known 'n'. The second, that it may be
|
||||
% Binomial but 'n' is unknown and should be estimated. Note the second
|
||||
% scenario may not yield a Binomial Distribution as the best fit, if
|
||||
% 'n' is estimated incorrectly. (Best to run example a couple times
|
||||
% to see effect)
|
||||
% data = binornd(10,.3,1e2,1);
|
||||
% [D1 PD1] = allfitdist(data,'n',10,'DISCRETE','PDF'); %Force binomial
|
||||
% [D2 PD2] = allfitdist(data,'DISCRETE','PDF'); %May be binomial
|
||||
% PD1{1}, PD2{1} %Compare distributions
|
||||
%
|
||||
|
||||
% Mike Sheppard
|
||||
% Last Modified: 17-Feb-2012
|
||||
|
||||
|
||||
|
||||
|
||||
%% Check Inputs
|
||||
if nargin == 0
|
||||
data = 10.^((normrnd(2,10,1e4,1))/10);
|
||||
sortby='BIC';
|
||||
varargin={'CDF'};
|
||||
end
|
||||
if nargin==1
|
||||
sortby='BIC';
|
||||
end
|
||||
sortbyname={'NLogL','BIC','AIC','AICc'};
|
||||
if ~any(ismember(lower(sortby),lower(sortbyname)))
|
||||
oldvar=sortby; %May be 'PDF' or 'CDF' or other commands
|
||||
if isempty(varargin)
|
||||
varargin={oldvar};
|
||||
else
|
||||
varargin=[oldvar varargin];
|
||||
end
|
||||
sortby='BIC';
|
||||
end
|
||||
if nargin < 2, sortby='BIC'; end
|
||||
distname={'beta', 'birnbaumsaunders', 'exponential', ...
|
||||
'extreme value', 'gamma', 'generalized extreme value', ...
|
||||
'generalized pareto', 'inversegaussian', 'logistic', 'loglogistic', ...
|
||||
'lognormal', 'nakagami', 'normal', ...
|
||||
'rayleigh', 'rician', 'tlocationscale', 'weibull'};
|
||||
if ~any(strcmpi(sortby,sortbyname))
|
||||
error('allfitdist:SortBy','Sorting must be either NLogL, BIC, AIC, or AICc');
|
||||
end
|
||||
%Input may be mixed of numeric and strings, find only strings
|
||||
vin=varargin;
|
||||
strs=find(cellfun(@(vs)ischar(vs),vin));
|
||||
vin(strs)=lower(vin(strs));
|
||||
%Next check to see if 'PDF' or 'CDF' is listed
|
||||
numplots=sum(ismember(vin(strs),{'pdf' 'cdf'}));
|
||||
if numplots>=2
|
||||
error('ALLFITDIST:PlotType','Either PDF or CDF must be given');
|
||||
end
|
||||
if numplots==1
|
||||
plotind=true; %plot indicator
|
||||
indxpdf=ismember(vin(strs),'pdf');
|
||||
plotpdf=any(indxpdf);
|
||||
indxcdf=ismember(vin(strs),'cdf');
|
||||
vin(strs(indxpdf|indxcdf))=[]; %Delete 'PDF' and 'CDF' in vin
|
||||
else
|
||||
plotind=false;
|
||||
end
|
||||
%Check to see if discrete
|
||||
strs=find(cellfun(@(vs)ischar(vs),vin));
|
||||
indxdis=ismember(vin(strs),'discrete');
|
||||
discind=false;
|
||||
if any(indxdis)
|
||||
discind=true;
|
||||
distname={'binomial', 'negative binomial', 'poisson'};
|
||||
vin(strs(indxdis))=[]; %Delete 'DISCRETE' in vin
|
||||
end
|
||||
strs=find(cellfun(@(vs)ischar(vs),vin));
|
||||
n=numel(data); %Number of data points
|
||||
data = data(:);
|
||||
D=[];
|
||||
%Check for NaN's to delete
|
||||
deldatanan=isnan(data);
|
||||
%Check to see if frequency is given
|
||||
indxf=ismember(vin(strs),'frequency');
|
||||
if any(indxf)
|
||||
freq=vin{1+strs((indxf))}; freq=freq(:);
|
||||
if numel(freq)~=numel(data)
|
||||
error('ALLFITDIST:PlotType','Matrix dimensions must agree');
|
||||
end
|
||||
delfnan=isnan(freq);
|
||||
data(deldatanan|delfnan)=[]; freq(deldatanan|delfnan)=[];
|
||||
%Save back into vin
|
||||
vin{1+strs((indxf))}=freq;
|
||||
else
|
||||
data(deldatanan)=[];
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
%% Run through all distributions in FITDIST function
|
||||
warning('off','all'); %Turn off all future warnings
|
||||
for indx=1:length(distname)
|
||||
try
|
||||
dname=distname{indx};
|
||||
switch dname
|
||||
case 'binomial'
|
||||
PD=fitbinocase(data,vin,strs); %Special case
|
||||
case 'generalized pareto'
|
||||
PD=fitgpcase(data,vin,strs); %Special case
|
||||
otherwise
|
||||
%Built-in distribution using FITDIST
|
||||
PD = fitdist(data,dname,vin{:});
|
||||
end
|
||||
|
||||
NLL=PD.NLogL; % -Log(L)
|
||||
%If NLL is non-finite number, produce error to ignore distribution
|
||||
if ~isfinite(NLL)
|
||||
error('non-finite NLL');
|
||||
end
|
||||
num=length(D)+1;
|
||||
PDs(num) = {PD}; %#ok<*AGROW>
|
||||
k=numel(PD.Params); %Number of parameters
|
||||
D(num).DistName=PD.DistName;
|
||||
D(num).NLogL=NLL;
|
||||
D(num).BIC=-2*(-NLL)+k*log(n);
|
||||
D(num).AIC=-2*(-NLL)+2*k;
|
||||
D(num).AICc=(D(num).AIC)+((2*k*(k+1))/(n-k-1));
|
||||
D(num).ParamNames=PD.ParamNames;
|
||||
D(num).ParamDescription=PD.ParamDescription;
|
||||
D(num).Params=PD.Params;
|
||||
D(num).Paramci=PD.paramci;
|
||||
D(num).ParamCov=PD.ParamCov;
|
||||
D(num).Support=PD.Support;
|
||||
catch err %#ok<NASGU>
|
||||
%Ignore distribution
|
||||
end
|
||||
end
|
||||
warning('on','all'); %Turn back on warnings
|
||||
if numel(D)==0
|
||||
error('ALLFITDIST:NoDist','No distributions were found');
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
%% Sort distributions
|
||||
indx1=1:length(D); %Identity Map
|
||||
sortbyindx=find(strcmpi(sortby,sortbyname));
|
||||
switch sortbyindx
|
||||
case 1
|
||||
[~,indx1]=sort([D.NLogL]);
|
||||
case 2
|
||||
[~,indx1]=sort([D.BIC]);
|
||||
case 3
|
||||
[~,indx1]=sort([D.AIC]);
|
||||
case 4
|
||||
[~,indx1]=sort([D.AICc]);
|
||||
end
|
||||
%Sort
|
||||
D=D(indx1); PD = PDs(indx1);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
%% Plot if requested
|
||||
if plotind;
|
||||
plotfigs(data,D,PD,vin,strs,plotpdf,discind)
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
function PD=fitbinocase(data,vin,strs)
|
||||
%% Special Case for Binomial
|
||||
% 'n' is estimated if not given
|
||||
vinbino=vin;
|
||||
%Check to see if 'n' is given
|
||||
indxn=any(ismember(vin(strs),'n'));
|
||||
%Check to see if 'frequency' is given
|
||||
indxfreq=ismember(vin(strs),'frequency');
|
||||
if ~indxn
|
||||
%Use Method of Moment estimator
|
||||
%E[x]=np, V[x]=np(1-p) -> nhat=E/(1-(V/E));
|
||||
if isempty(indxfreq)||~any(indxfreq)
|
||||
%Raw data
|
||||
mnx=mean(data);
|
||||
nhat=round(mnx/(1-(var(data)/mnx)));
|
||||
else
|
||||
%Frequency data
|
||||
freq=vin{1+strs(indxfreq)};
|
||||
m1=dot(data,freq)/sum(freq);
|
||||
m2=dot(data.^2,freq)/sum(freq);
|
||||
mnx=m1; vx=m2-(m1^2);
|
||||
nhat=round(mnx/(1-(vx/mnx)));
|
||||
end
|
||||
%If nhat is negative, use maximum value of data
|
||||
if nhat<=0, nhat=max(data(:)); end
|
||||
vinbino{end+1}='n'; vinbino{end+1}=nhat;
|
||||
end
|
||||
PD = fitdist(data,'binomial',vinbino{:});
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
function PD=fitgpcase(data,vin,strs)
|
||||
%% Special Case for Generalized Pareto
|
||||
% 'theta' is estimated if not given
|
||||
vingp=vin;
|
||||
%Check to see if 'theta' is given
|
||||
indxtheta=any(ismember(vin(strs),'theta'));
|
||||
if ~indxtheta
|
||||
%Use minimum value for theta, minus small part
|
||||
thetahat=min(data(:))-10*eps;
|
||||
vingp{end+1}='theta'; vingp{end+1}=thetahat;
|
||||
end
|
||||
PD = fitdist(data,'generalized pareto',vingp{:});
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
function plotfigs(data,D,PD,vin,strs,plotpdf,discind)
|
||||
%Plot functionality for continuous case due to Jonathan Sullivan
|
||||
%Modified by author for discrete case
|
||||
|
||||
%Maximum number of distributions to include
|
||||
%max_num_dist=Inf; %All valid distributions
|
||||
max_num_dist=4;
|
||||
|
||||
%Check to see if frequency is given
|
||||
indxf=ismember(vin(strs),'frequency');
|
||||
if any(indxf)
|
||||
freq=vin{1+strs((indxf))};
|
||||
end
|
||||
|
||||
figure
|
||||
|
||||
%% Probability Density / Mass Plot
|
||||
if plotpdf
|
||||
if ~discind
|
||||
%Continuous Data
|
||||
nbins = max(min(length(data)./10,100),50);
|
||||
xi = linspace(min(data),max(data),nbins);
|
||||
dx = mean(diff(xi));
|
||||
xi2 = linspace(min(data),max(data),nbins*10)';
|
||||
fi = histc(data,xi-dx);
|
||||
fi = fi./sum(fi)./dx;
|
||||
inds = 1:min([max_num_dist,numel(PD)]);
|
||||
ys = cellfun(@(PD) pdf(PD,xi2),PD(inds),'UniformOutput',0);
|
||||
ys = cat(2,ys{:});
|
||||
bar(xi,fi,'FaceColor',[160 188 254]/255,'EdgeColor','k');
|
||||
hold on;
|
||||
plot(xi2,ys,'LineWidth',1.5)
|
||||
legend(['empirical',{D(inds).DistName}],'Location','NE')
|
||||
xlabel('Value');
|
||||
ylabel('Probability Density');
|
||||
title('Probability Density Function');
|
||||
grid on
|
||||
else
|
||||
%Discrete Data
|
||||
xi2=min(data):max(data);
|
||||
%xi2=unique(x)'; %If only want observed x-values to be shown
|
||||
indxf=ismember(vin(strs),'frequency');
|
||||
if any(indxf)
|
||||
fi=zeros(size(xi2));
|
||||
fi((ismember(xi2,data)))=freq; fi=fi'./sum(fi);
|
||||
else
|
||||
fi=histc(data,xi2); fi=fi./sum(fi);
|
||||
end
|
||||
inds = 1:min([max_num_dist,numel(PD)]);
|
||||
ys = cellfun(@(PD) pdf(PD,xi2),PD(inds),'UniformOutput',0);
|
||||
ys=cat(1,ys{:})';
|
||||
bar(xi2,[fi ys]);
|
||||
legend(['empirical',{D(inds).DistName}],'Location','NE')
|
||||
xlabel('Value');
|
||||
ylabel('Probability Mass');
|
||||
title('Probability Mass Function');
|
||||
grid on
|
||||
end
|
||||
else
|
||||
|
||||
%Cumulative Distribution
|
||||
if ~discind
|
||||
%Continuous Data
|
||||
[fi xi] = ecdf(data);
|
||||
inds = 1:min([max_num_dist,numel(PD)]);
|
||||
ys = cellfun(@(PD) cdf(PD,xi),PD(inds),'UniformOutput',0);
|
||||
ys = cat(2,ys{:});
|
||||
if max(xi)/min(xi) > 1e4; lgx = true; else lgx = false; end
|
||||
subplot(2,1,1)
|
||||
if lgx
|
||||
semilogx(xi,fi,'k',xi,ys)
|
||||
else
|
||||
plot(xi,fi,'k',xi,ys)
|
||||
end
|
||||
legend(['empirical',{D(inds).DistName}],'Location','NE')
|
||||
xlabel('Value');
|
||||
ylabel('Cumulative Probability');
|
||||
title('Cumulative Distribution Function');
|
||||
grid on
|
||||
subplot(2,1,2)
|
||||
y = 1.1*bsxfun(@minus,ys,fi);
|
||||
if lgx
|
||||
semilogx(xi,bsxfun(@minus,ys,fi))
|
||||
else
|
||||
plot(xi,bsxfun(@minus,ys,fi))
|
||||
end
|
||||
ybnds = max(abs(y(:)));
|
||||
ax = axis;
|
||||
axis([ax(1:2) -ybnds ybnds]);
|
||||
legend({D(inds).DistName},'Location','NE')
|
||||
xlabel('Value');
|
||||
ylabel('Error');
|
||||
title('CDF Error');
|
||||
grid on
|
||||
else
|
||||
%Discrete Data
|
||||
indxf=ismember(vin(strs),'frequency');
|
||||
if any(indxf)
|
||||
[fi xi] = ecdf(data,'frequency',freq);
|
||||
else
|
||||
[fi xi] = ecdf(data);
|
||||
end
|
||||
%Check unique xi, combine fi
|
||||
[xi,ign,indx]=unique(xi); %#ok<ASGLU>
|
||||
fi=accumarray(indx,fi);
|
||||
inds = 1:min([max_num_dist,numel(PD)]);
|
||||
ys = cellfun(@(PD) cdf(PD,xi),PD(inds),'UniformOutput',0);
|
||||
ys=cat(2,ys{:});
|
||||
subplot(2,1,1)
|
||||
stairs(xi,[fi ys]);
|
||||
legend(['empirical',{D(inds).DistName}],'Location','NE')
|
||||
xlabel('Value');
|
||||
ylabel('Cumulative Probability');
|
||||
title('Cumulative Distribution Function');
|
||||
grid on
|
||||
subplot(2,1,2)
|
||||
y = 1.1*bsxfun(@minus,ys,fi);
|
||||
stairs(xi,bsxfun(@minus,ys,fi))
|
||||
ybnds = max(abs(y(:)));
|
||||
ax = axis;
|
||||
axis([ax(1:2) -ybnds ybnds]);
|
||||
legend({D(inds).DistName},'Location','NE')
|
||||
xlabel('Value');
|
||||
ylabel('Error');
|
||||
title('CDF Error');
|
||||
grid on
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -1,208 +0,0 @@
|
|||
% 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));
|
||||
for i = length(files):-1:1
|
||||
if (~contains(files(i).name, 'result') ||...
|
||||
contains(files(i).name, 'left') ||...
|
||||
contains(files(i).name, 'right') ||...
|
||||
contains(files(i).name, 'ap') ||...
|
||||
contains(files(i).name, 'down') ||...
|
||||
contains(files(i).name, 'up'))
|
||||
files(i) = [];
|
||||
targets(i) = [];
|
||||
else
|
||||
targets(i) = sscanf(files(i).name, 'result_%dcm.txt');
|
||||
end
|
||||
end
|
||||
[targets, orderI] = sort(targets);
|
||||
files = files(orderI);
|
||||
|
||||
median_result = zeros(1, length(files));
|
||||
mean_result = zeros(1, length(files));
|
||||
median_rssi = zeros(1, length(files));
|
||||
median_diststd = zeros(1, length(files));
|
||||
all_data = [];
|
||||
figure(1); clf; hold on;
|
||||
% figure(2); clf; hold on;
|
||||
for i = 1:length(files)
|
||||
filename = [folder, files(i).name];
|
||||
|
||||
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)))';
|
||||
rawRTTVar = str2double(table2array(data(:, 4)))';
|
||||
rawDist = str2double(table2array(data(:, 5)))';
|
||||
rawDistVar = str2double(table2array(data(:, 6)))';
|
||||
rssi = str2double(table2array(data(:, 7)))';
|
||||
time = str2double(table2array(data(:, 8)))';
|
||||
else
|
||||
% get rid of invalid data
|
||||
data(:, data(7, :) ~= 0) = [];
|
||||
data(:, data(9, :) < -1000) = [];
|
||||
rawDist = data(9, :);
|
||||
rawDistVar = zeros(size(rawDist));
|
||||
rssi = zeros(size(rawDist));
|
||||
end
|
||||
|
||||
mean_result(i) = mean(rawDist);
|
||||
median_result(i) = median(rawDist);
|
||||
median_diststd(i) = median(sqrt(rawDistVar));
|
||||
median_rssi(i) = median(rssi);
|
||||
|
||||
fprintf('distance: %d:\n', targets(i));
|
||||
fprintf('* mean: %.2f (uncalibrated)\n', mean_result(i));
|
||||
fprintf('* median: %.2f (uncalibrated)\n', median_result(i));
|
||||
fprintf('* std: %.2f (uncalibrated)\n', std(rawDist));
|
||||
|
||||
figure(1); cdfplot(rawDist);
|
||||
diffs_each = param_linear(1) * rawDist + param_linear(2) - targets(i);
|
||||
pd = fitdist(diffs_each(~isnan(diffs_each))', 'Normal');
|
||||
fit_pd_each(i) = pd;
|
||||
% figure(2);
|
||||
% scatter3(...
|
||||
% sqrt(rawDistVar),...
|
||||
% rssi,...
|
||||
% rawDist - targets(i));
|
||||
|
||||
all_data = [...
|
||||
all_data,...
|
||||
[rawDist;...
|
||||
targets(i) * ones(1, size(rawDist, 2));...
|
||||
sqrt(rawDistVar);...
|
||||
rssi
|
||||
]...
|
||||
];
|
||||
end
|
||||
|
||||
% % shuffle
|
||||
% shuffled_data = all_data(:, randperm(size(all_data, 2)));
|
||||
%
|
||||
% % 10-fold cross validation
|
||||
% step = floor(size(shuffled_data, 2) / 20);
|
||||
% params = zeros(2, 20);
|
||||
% mse = zeros(1, 20);
|
||||
% for i = 1:20
|
||||
% from = step * (i - 1) + 1;
|
||||
% to = step * i;
|
||||
% train_data = shuffled_data;
|
||||
% test_data = train_data(:, from:to);
|
||||
% train_data(:, from:to) = [];
|
||||
% params(:, i) = polyfit(train_data(1, :), train_data(2, :), 1);
|
||||
% test_est = params(1, i) * test_data(1, :) + params(2, i);
|
||||
% mse(i) = sum((test_est - test_data(2, :)).^2) / size(test_data, 2);
|
||||
% end
|
||||
|
||||
% param(1) = sum(params(1, :)) / size(mse, 2);
|
||||
% % mse ./ sum(mse) * params(1, :)';
|
||||
% param(2) = sum(params(2, :)) / size(mse, 2);
|
||||
% % mse ./ sum(mse) * params(2, :)';
|
||||
% validated_fit_data = param(1) * all_data(1, :) + param(2);
|
||||
% mstd_1 = sqrt(sum((validated_fit_data - all_data(2, :)).^2) /...
|
||||
% size(all_data, 2));
|
||||
|
||||
|
||||
figure(3); clf; hold on;
|
||||
scatter(all_data(1, :), all_data(2, :), 'b.');
|
||||
plot(median_result, targets, 'r', 'LineWidth', 2)
|
||||
|
||||
% linear fit
|
||||
param_linear = polyfit(all_data(1, :), all_data(2, :), 1);
|
||||
data_linear = param_linear(1) * all_data(1, :) + param_linear(2);
|
||||
mstd_linear = sqrt(...
|
||||
sum((data_linear - all_data(2, :)).^2) / size(all_data, 2));
|
||||
scatter(all_data(1, :), data_linear, 'c.');
|
||||
|
||||
|
||||
diffs = data_linear - all_data(2, :); diffs_std = [];
|
||||
figure(4); clf; hold on;
|
||||
cdfplot(diffs)
|
||||
% for i = 1:size(targets, 2)
|
||||
% diffs_std(i) = std(diffs(all_data(2, :) == targets(i)));
|
||||
% % pause;
|
||||
% end
|
||||
% scatter(median_rssi, diffs_std)
|
||||
|
||||
figure(3);
|
||||
% parabolic fit
|
||||
param_parabolic = polyfit(all_data(1, :), all_data(2, :), 2);
|
||||
data_parabolic = ...
|
||||
param_parabolic(1) * all_data(1, :).^2 +...
|
||||
param_parabolic(2) * all_data(1, :) + ...
|
||||
param_parabolic(3);
|
||||
mstd_parabolic = sqrt(...
|
||||
sum((data_parabolic - all_data(2, :)).^2) / size(all_data, 2));
|
||||
scatter(all_data(1, :), data_parabolic, 'g.');
|
||||
|
||||
xlabel('Raw Distance');
|
||||
ylabel('Target Distance');
|
||||
legend('all data', 'median val', 'linear fit', 'parabolic fit', 'location', 'best')
|
||||
|
||||
fprintf('Std Err:\n');
|
||||
fprintf(' linear mode: %.6f\n', mstd_linear);
|
||||
fprintf(' parabolic mode: %.6f\n', mstd_parabolic);
|
||||
|
||||
|
||||
|
||||
|
||||
%
|
||||
% diffs = [];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_1517706485.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_1517706584.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_1517706685.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_1517706784.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_1517706881.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_1517706976.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_1517707172.txt')];
|
||||
|
||||
%
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_r_1517706433.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_r_1517706537.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_r_1517706636.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_r_1517706737.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_r_1517706835.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_r_1517706930.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_outdoor/result_walking_3800_to_100cm_r_1517707073.txt')];
|
||||
% pd = fitdist(diffs(~isnan(diffs))', 'Normal');
|
||||
% prob_fit = cdf(pd, diffs(~isnan(diffs))');
|
||||
% figure(10); clf; hold on;
|
||||
% scatter(diffs(~isnan(diffs))', prob_fit);
|
||||
% cdfplot(diffs); xlim([-200, 200])
|
||||
% xlabel('Distance Err (cm)')
|
||||
% title('Distribution of Distance Err When Walking')
|
||||
% legend('fitted \mu=53.5043, \sigma=72.1852', 'actual dist error', 'location', 'best')
|
||||
|
||||
% %
|
||||
% diffs = [];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_indoor/result_walking_3600_to_100cm_1517693155.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_indoor/result_walking_3600_to_100cm_1517693287.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_indoor/result_walking_3600_to_100cm_1517693408.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_indoor/result_walking_3600_to_100cm_1517693534.txt')];
|
||||
%
|
||||
% diffs = [];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_indoor/result_walking_3600_to_100cm_r_1517693224.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_indoor/result_walking_3600_to_100cm_r_1517693350.txt')];
|
||||
% diffs = [diffs, calibration_walking('calibration_data/walking_indoor/result_walking_3600_to_100cm_r_1517693475.txt')];
|
||||
|
||||
|
||||
% pd = fitdist(diffs(~isnan(diffs))', 'Normal');
|
||||
% prob_fit = cdf(pd, diffs(~isnan(diffs))');
|
||||
% figure(11); clf; hold on;
|
||||
% scatter(diffs(~isnan(diffs))', prob_fit);
|
||||
% cdfplot(diffs); xlim([-200, 200])
|
||||
% xlabel('Distance Err (cm)')
|
||||
% title('Distribution of Distance Err When Walking')
|
||||
% legend('fitted \mu=28.8055, \sigma=50.1228', 'actual dist error', 'location', 'best')
|
||||
|
|
@ -1,105 +0,0 @@
|
|||
clear all;
|
||||
|
||||
folder = 'calibration_data/new_indoor/';
|
||||
|
||||
files = dir(folder); files = files(3:end); % remove . and ..
|
||||
files_left = files;
|
||||
files_right = files;
|
||||
targets = zeros(1, length(files));
|
||||
for i = length(files):-1:1
|
||||
if contains(files(i).name, 'left') || contains(files(i).name, 'right')
|
||||
if contains(files(i).name, 'left')
|
||||
files_right(i) = [];
|
||||
% remove only once
|
||||
targets(i) = sscanf(files(i).name, 'result_%dcm.txt');
|
||||
else
|
||||
targets(i) = [];
|
||||
files_left(i) = [];
|
||||
end
|
||||
else
|
||||
files_right(i) = [];
|
||||
files_left(i) = [];
|
||||
targets(i) = [];
|
||||
end
|
||||
end
|
||||
for i = length(files):-1:1
|
||||
target = sscanf(files(i).name, 'result_%dcm.txt');
|
||||
if isempty(target)
|
||||
continue
|
||||
end
|
||||
if ~ismember(target, targets) || ...
|
||||
contains(files(i).name, 'left') || ...
|
||||
contains(files(i).name, 'right') || ...
|
||||
contains(files(i).name, 'ap') || ...
|
||||
contains(files(i).name, 'down') || ...
|
||||
contains(files(i).name, 'up')
|
||||
files(i) = [];
|
||||
end
|
||||
end
|
||||
[targets, orderI] = sort(targets);
|
||||
files_left = files_left(orderI);
|
||||
files_right = files_right(orderI);
|
||||
files = files(orderI);
|
||||
|
||||
medians = zeros(3, length(targets));
|
||||
figure(1); clf;
|
||||
subplot(2, 1, 1); hold on;
|
||||
for i = 1:length(targets)
|
||||
% normal
|
||||
filename = [folder, files(i).name];
|
||||
data = readtable(filename, 'ReadVariableNames', 0);
|
||||
if isempty(data)
|
||||
continue
|
||||
end
|
||||
data = data(2:end, :);
|
||||
rawDist_ori = str2double(table2array(data(:, 5)))';
|
||||
rawDistVar_ori = str2double(table2array(data(:, 6)))';
|
||||
rssi_ori = str2double(table2array(data(:, 7)))';
|
||||
time_ori = str2double(table2array(data(:, 8)))';
|
||||
medians(1, i) = median(rawDist_ori);
|
||||
% left
|
||||
filename = [folder, files_left(i).name];
|
||||
data = readtable(filename, 'ReadVariableNames', 0);
|
||||
if isempty(data)
|
||||
continue
|
||||
end
|
||||
data = data(2:end, :);
|
||||
rawDist_left = str2double(table2array(data(:, 5)))';
|
||||
rawDistVar_left = str2double(table2array(data(:, 6)))';
|
||||
rssi_left = str2double(table2array(data(:, 7)))';
|
||||
time_left = str2double(table2array(data(:, 8)))';
|
||||
medians(2, i) = median(rawDist_left);
|
||||
% right
|
||||
filename = [folder, files_right(i).name];
|
||||
data = readtable(filename, 'ReadVariableNames', 0);
|
||||
if isempty(data)
|
||||
continue
|
||||
end
|
||||
data = data(2:end, :);
|
||||
rawDist_right = str2double(table2array(data(:, 5)))';
|
||||
rawDistVar_right = str2double(table2array(data(:, 6)))';
|
||||
rssi_right = str2double(table2array(data(:, 7)))';
|
||||
time_right = str2double(table2array(data(:, 8)))';
|
||||
medians(3, i) = median(rawDist_right);
|
||||
|
||||
% plot rssi relation
|
||||
subplot(2, 1, 1);
|
||||
scatter(rawDist_ori, rssi_ori, 'ro');
|
||||
scatter(rawDist_left, rssi_left, 'cs');
|
||||
scatter(rawDist_right, rssi_right, 'b*');
|
||||
end
|
||||
|
||||
subplot(2, 1, 1);
|
||||
title('Signal propagation variation');
|
||||
legend('Face2face', 'Left2Face', 'Right2Face', 'Location', 'best');
|
||||
xlabel('Reported Raw Distance (cm)');
|
||||
ylabel('RSSI (dBm)');
|
||||
|
||||
subplot(2, 1, 2); hold on;
|
||||
title('Validation: No Significant Variation in Orientation :)');
|
||||
scatter(targets, medians(1, :), 'ro', 'LineWidth', 2);
|
||||
scatter(targets, medians(2, :), 'cs', 'LineWidth', 2);
|
||||
scatter(targets, medians(3, :), 'b*', 'LineWidth', 2);
|
||||
legend('Face2face', 'Left2Face', 'Right2Face', 'Location', 'best');
|
||||
xlabel('Groundtruth (cm)');
|
||||
ylabel({'(Median) Reported', 'Raw Distance (cm)'});
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
function diffs = calibration_walking(filename)
|
||||
startI = 3600;
|
||||
endI = 100;
|
||||
if contains(filename, '_r_')
|
||||
startI = 100;
|
||||
endI = 3600;
|
||||
end
|
||||
% param_linear = [0.6927, 400.3157]; % medical_try_0b
|
||||
% param_linear = [0.7927,483.3157]; % medical_try_1f
|
||||
param_linear = [0.8927,553.3157]; % outdoor
|
||||
% param_linear = [0.9376, 558.0551]; % indoor
|
||||
data = readtable(filename, 'ReadVariableNames', 0);
|
||||
data = data(2:end, :);
|
||||
caliDist = str2double(table2array(data(:, 2)))';
|
||||
rawRTT = str2double(table2array(data(:, 3)))';
|
||||
rawRTTVar = str2double(table2array(data(:, 4)))';
|
||||
rawDist = str2double(table2array(data(:, 5)))';
|
||||
rawDistVar = str2double(table2array(data(:, 6)))';
|
||||
rssi = str2double(table2array(data(:, 7)))';
|
||||
time = str2double(table2array(data(:, 8)))';
|
||||
% clear invalid data
|
||||
logistics = rawDist < -1000 | rawDist > 10000 | isnan(time);
|
||||
caliDist(logistics) = [];
|
||||
rawRTT(logistics) = [];
|
||||
rawRTTVar(logistics) = [];
|
||||
rawDist(logistics) = [];
|
||||
rawDistVar(logistics) = [];
|
||||
rssi(logistics) = [];
|
||||
time(logistics) = [];
|
||||
% normalize time
|
||||
time = (time - time(1)) ./ (time(end) - time(1));
|
||||
|
||||
% rawDist = movmean(rawDist, 10, 'omitnan');
|
||||
% rssi = movmean(rssi, 10, 'omitnan');
|
||||
% for i = size(caliDist, 2) - 3: -4: 1
|
||||
% rawDist(i) = nanmean(rawDist(i: i + 3));
|
||||
% rawDist(i + 1: i + 3) = [];
|
||||
% rssi(i) = nanmean(rssi(i: i + 3));
|
||||
% rssi(i + 1: i + 3) = [];
|
||||
% time(i + 1: i + 3) = [];
|
||||
% end
|
||||
|
||||
dist = rawDist * param_linear(1) + param_linear(2);
|
||||
targets = (time - time(1)) * (endI - startI) / (time(end) - time(1)) + startI;
|
||||
diffs = (targets - dist);
|
||||
fprintf("** mean diff: %.4f cm\n", nanmean(diffs));
|
||||
fprintf("** median diff: %.4f cm\n", nanmedian(diffs));
|
||||
|
||||
figure(1); % clf;
|
||||
scatter3(time, dist, rssi); hold on; scatter(time, dist, '.')
|
||||
% plot([time(1), time(end)], [startI, endI]);
|
||||
view([0, 90]);
|
||||
% if contains(filename, '_r_')
|
||||
% title('back')
|
||||
% figure(9); hold on;
|
||||
% plot(flip((time - time(1)) ./ (time(end) - time(1))), rssi)
|
||||
% else
|
||||
% title('front')
|
||||
% figure(9); hold on;
|
||||
% plot(((time - time(1)) ./ (time(end) - time(1))), rssi)
|
||||
% end
|
||||
|
||||
% figure(2); clf; hold on;
|
||||
% pd = fitdist(diffs(~isnan(diffs))', 'Normal')
|
||||
% prob_fit = cdf(pd, diffs(~isnan(diffs))');
|
||||
% scatter(diffs(~isnan(diffs))', prob_fit);
|
||||
% cdfplot(diffs);
|
||||
% % histogram(diffs, 'BinWidth', 1, 'Normalization', 'pdf');
|
||||
|
||||
end
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
{
|
||||
"34:f6:4b:5e:69:1f": {
|
||||
"bw": 20,
|
||||
"cf": 2462,
|
||||
"spb": 255,
|
||||
"retries": 3,
|
||||
"location": "500,0"
|
||||
},
|
||||
"34:f6:4b:5e:69:0b": {
|
||||
"bw": 20,
|
||||
"cf": 2437,
|
||||
"spb": 255,
|
||||
"retries": 3,
|
||||
"location": "0,0"
|
||||
}
|
||||
}
|
||||
|
|
@ -1,97 +0,0 @@
|
|||
% pd = makedist('normal', 'mu', 0, 'sigma', 53.8836); % cm
|
||||
|
||||
pd = makedist('tlocationscale', 'mu', 0, 'sigma', 42.8875, 'nu', 5.3095);
|
||||
|
||||
% params
|
||||
plotflag = 0;
|
||||
samples = 100000;
|
||||
farthest_dist = [500, 2000]; % cm
|
||||
devLocations = [0, 0;
|
||||
500, 0;
|
||||
500, 2000;
|
||||
0, 2000];
|
||||
|
||||
% setup
|
||||
for i = size(devLocations, 1):-1:1
|
||||
for j = size(devLocations, 1):-1:(i+1)
|
||||
diff = devLocations(i, :) - devLocations(j, :);
|
||||
pairwiseDist(i, j) = sqrt(sum(diff.^2));
|
||||
end
|
||||
end
|
||||
targetLocs = rand(samples, 2) .* farthest_dist;
|
||||
% targetLocs = [250, 100; 250, 200; 250, 300; 250, 400; 250, 500];
|
||||
% samples = size(targetLocs, 1);
|
||||
trueDist = zeros(size(devLocations, 1), samples);
|
||||
estDist = zeros(size(devLocations, 1), samples);
|
||||
|
||||
% % estimate measurements
|
||||
for i = 1:size(devLocations, 1)
|
||||
trueDist(i, :) = sqrt(sum((targetLocs - devLocations(i, :)).^2, 2));
|
||||
if isa(pd, 'prob.NormalDistribution')
|
||||
randvar_vec = random(pd, size(trueDist(i, :)));
|
||||
else
|
||||
signs = (rand(size(trueDist(i, :))) < 0.5) * 2 - 1;
|
||||
randvar_vec = random(pd, size(trueDist(i, :))) .* signs;
|
||||
end
|
||||
estDist(i, :) = trueDist(i, :) + randvar_vec;
|
||||
end
|
||||
|
||||
% estimate locations
|
||||
estLocs = zeros(size(targetLocs));
|
||||
if plotflag
|
||||
figure(3); clf; hold on;
|
||||
scatter(devLocations(:, 1), devLocations(:, 2), 'b*');
|
||||
end
|
||||
for i = 1:samples
|
||||
points = [];
|
||||
for j = 1:size(devLocations, 1)
|
||||
for k = (j+1):size(devLocations, 1)
|
||||
d = pairwiseDist(j, k);
|
||||
if (d > estDist(j, i) + estDist(k, i)) ||...
|
||||
(d < abs(estDist(j, i) - estDist(k, i)))
|
||||
continue
|
||||
end
|
||||
a = (estDist(j, i) * estDist(j, i) -...
|
||||
estDist(k, i) * estDist(k, i) +...
|
||||
d * d) / (2 * d);
|
||||
h = sqrt(estDist(j, i) * estDist(j, i) - a * a);
|
||||
x0 = devLocations(j, 1) +...
|
||||
a * (devLocations(k, 1) - devLocations(j, 1)) / d;
|
||||
y0 = devLocations(j, 2) +...
|
||||
a * (devLocations(k, 2) - devLocations(j, 2)) / d;
|
||||
rx = -(devLocations(k, 2) - devLocations(j, 2)) * (h / d);
|
||||
ry = -(devLocations(k, 1) - devLocations(j, 1)) * (h / d);
|
||||
points = [points; x0 + rx, y0 - ry; x0 - rx, y0 + ry];
|
||||
end
|
||||
end
|
||||
if ~isempty(points) && size(devLocations, 1) == 2
|
||||
if devLocations(1, 2) == devLocations(2, 2)
|
||||
points(points(:, 2) < devLocations(1, 2), :) = [];
|
||||
elseif devLocations(1, 1) == devLocations(2, 1)
|
||||
points(points(:, 1) < devLocations(1, 1), :) = [];
|
||||
end
|
||||
end
|
||||
estLoc = median(points, 1);
|
||||
if isempty(estLoc)
|
||||
estLocs(i, :) = nan;
|
||||
else
|
||||
estLocs(i, :) = estLoc;
|
||||
if plotflag
|
||||
scatter(targetLocs(i, 1), targetLocs(i, 2), 'ks');
|
||||
scatter(estLocs(i, 1), estLocs(i, 2), 'r.');
|
||||
pause(0.001)
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
logistics = isnan(estLocs(:, 1));
|
||||
estLocs(logistics, :) = [];
|
||||
targetLocs(logistics, :) = [];
|
||||
fprintf('* %d cannot est locations\n', sum(logistics));
|
||||
|
||||
err = sqrt(sum((estLocs - targetLocs).^2, 2));
|
||||
figure(4); hold on; cdfplot(err);
|
||||
|
||||
fitresult = allfitdist(err, 'pdf');
|
||||
% essentially, we are getting a gamma distribution in generall
|
||||
% the result should be validated in real-life experiments also
|
||||
|
|
@ -1,167 +0,0 @@
|
|||
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
|
||||
|
|
@ -317,8 +317,7 @@ static void parse_ftm_results(struct nlattr **attrs, int status,
|
|||
struct nlattr *tb[NL80211_FTM_RESP_ENTRY_ATTR_MAX + 1];
|
||||
unsigned char bssid[ETH_ALEN];
|
||||
char macbuf[6 * 3];
|
||||
long long int rtt, rtt_var, dist, dist_var;
|
||||
signed int rssi;
|
||||
long long int rtt, dist;
|
||||
int err;
|
||||
|
||||
printf("FTM result! Status: %d\n", status);
|
||||
|
|
@ -353,22 +352,13 @@ static void parse_ftm_results(struct nlattr **attrs, int status,
|
|||
|
||||
mac_addr_n2a(macbuf, bssid);
|
||||
rtt = (long long int)nla_get_u64(tb[NL80211_FTM_RESP_ENTRY_ATTR_RTT]);
|
||||
rtt_var = tb[NL80211_FTM_RESP_ENTRY_ATTR_RTT_VAR] ?
|
||||
(long long int)nla_get_u64(
|
||||
tb[NL80211_FTM_RESP_ENTRY_ATTR_RTT_VAR]) : 0;
|
||||
dist = tb[NL80211_FTM_RESP_ENTRY_ATTR_DISTANCE] ?
|
||||
(long long int)nla_get_u64(tb[NL80211_FTM_RESP_ENTRY_ATTR_DISTANCE]) :
|
||||
rtt * SOL_CM_PSEC;
|
||||
dist_var = tb[NL80211_FTM_RESP_ENTRY_ATTR_DISTANCE_VAR] ?
|
||||
(long long int)nla_get_u64(
|
||||
tb[NL80211_FTM_RESP_ENTRY_ATTR_DISTANCE_VAR]) : 0;
|
||||
rssi = tb[NL80211_FTM_RESP_ENTRY_ATTR_RSSI] ?
|
||||
(signed int)nla_get_s8(tb[NL80211_FTM_RESP_ENTRY_ATTR_RSSI]) : 0;
|
||||
|
||||
printf(
|
||||
"Target: %s, status: %d, rtt: %lld (±%lld) psec, distance: %lld (±%lld) cm, rssi: %d dBm",
|
||||
macbuf, nla_get_u8(tb[NL80211_FTM_RESP_ENTRY_ATTR_STATUS]),
|
||||
rtt, rtt_var, dist, dist_var, rssi);
|
||||
printf("Target: %s, status: %d, rtt: %lld psec, distance: %lld cm",
|
||||
macbuf, nla_get_u8(tb[NL80211_FTM_RESP_ENTRY_ATTR_STATUS]), rtt,
|
||||
dist);
|
||||
|
||||
if (tb[NL80211_FTM_RESP_ENTRY_ATTR_LCI])
|
||||
iw_hexdump("LCI",
|
||||
93
iw_parser.py
93
iw_parser.py
|
|
@ -1,93 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
|
||||
import os
|
||||
import re
|
||||
import argparse
|
||||
|
||||
from numpy import min, max, median, mean, std
|
||||
from numpy.random import choice
|
||||
|
||||
|
||||
def wrapper(args):
|
||||
if not args['filepath'] or not os.path.isfile(args['filepath']):
|
||||
return
|
||||
results = []
|
||||
regex = (
|
||||
r"Target: (([0-9a-f]{2}:*){6}), " +
|
||||
r"status: ([0-9]), rtt: ([0-9\-]+) psec, " +
|
||||
r"distance: ([0-9\-]+) cm"
|
||||
)
|
||||
regex_new = (
|
||||
r"Target: (([0-9a-f]{2}:*){6}), " +
|
||||
r"status: ([0-9]), rtt: ([0-9\-]+) \(±([0-9\-]+)\) psec, " +
|
||||
r"distance: ([0-9\-]+) \(±([0-9\-]+)\) cm, rssi: ([0-9\-]+) dBm"
|
||||
)
|
||||
with open(args['filepath']) as f:
|
||||
data_ori = f.readlines()
|
||||
if args['sample'] is None:
|
||||
data = data_ori
|
||||
else:
|
||||
data = choice(data_ori, size=args['sample'], replace=False)
|
||||
for line in data:
|
||||
match = re.search(regex_new, line)
|
||||
if match:
|
||||
mac = match.group(1)
|
||||
status = int(match.group(3))
|
||||
rtt = int(match.group(4))
|
||||
rtt_var = int(match.group(5))
|
||||
raw_distance = int(match.group(6))
|
||||
raw_distance_var = int(match.group(7))
|
||||
rssi = int(match.group(8))
|
||||
else:
|
||||
match = re.search(regex, line)
|
||||
if match:
|
||||
mac = match.group(1)
|
||||
status = int(match.group(3))
|
||||
rtt = int(match.group(4))
|
||||
raw_distance = int(match.group(5))
|
||||
else:
|
||||
continue
|
||||
if status is not 0 or raw_distance < -1000:
|
||||
continue
|
||||
results.append(raw_distance * args['cali'][0] + args['cali'][1])
|
||||
print('statics of results')
|
||||
print('* num of valid data: {0}'.format(len(results)))
|
||||
print('* min: {0:.2f}cm'.format(min(results)))
|
||||
print('* max: {0:.2f}cm'.format(max(results)))
|
||||
print('* mean: {0:.2f}cm'.format(mean(results)))
|
||||
print('* median: {0:.2f}cm'.format(median(results)))
|
||||
print('* std: {0:.2f}cm'.format(std(results)))
|
||||
|
||||
|
||||
def main():
|
||||
p = argparse.ArgumentParser(description='iw parser')
|
||||
p.add_argument(
|
||||
'filepath',
|
||||
help="input file path for result"
|
||||
)
|
||||
p.add_argument(
|
||||
'--cali',
|
||||
nargs=2,
|
||||
# default=(0.9376, 558.0551), # indoor
|
||||
default=(0.8927, 553.3157), # outdoor
|
||||
type=float,
|
||||
help="calibrate final result"
|
||||
)
|
||||
p.add_argument(
|
||||
'--sample',
|
||||
default=None,
|
||||
type=int,
|
||||
help="if set (an integer), sample data for accuracy testing"
|
||||
)
|
||||
try:
|
||||
args = vars(p.parse_args())
|
||||
except Exception as e:
|
||||
print(e)
|
||||
sys.exit()
|
||||
wrapper(args)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import os
|
||||
import time
|
||||
import argparse
|
||||
|
||||
|
||||
def separateMAC(fp):
|
||||
results = {}
|
||||
nameLine = None
|
||||
startLine = None
|
||||
endLine = None
|
||||
with open(fp, 'r') as f:
|
||||
for line in f:
|
||||
if nameLine is None:
|
||||
nameLine = line
|
||||
continue
|
||||
if 'ff:ff:ff:ff:ff:ff' in line:
|
||||
if startLine is None:
|
||||
startLine = line
|
||||
elif endLine is None:
|
||||
endLine = line
|
||||
continue
|
||||
mac = line.split(',')[0]
|
||||
if mac not in results:
|
||||
print('* Find MAC: {0}'.format(mac))
|
||||
results[mac] = []
|
||||
results[mac].append(line)
|
||||
return results, nameLine, startLine, endLine
|
||||
|
||||
|
||||
def extract_each(fp):
|
||||
results, nameLine, startLine, endLine = separateMAC(fp)
|
||||
base, ext = os.path.splitext(fp)
|
||||
base = '_'.join(base.split('_')[:-1]) # remove time stamp
|
||||
t = int(time.time())
|
||||
for mac in results:
|
||||
identifier = mac.replace(':', '-')
|
||||
with open(
|
||||
'{0}_extract_{1}_{2}{3}'
|
||||
.format(base, identifier, t, ext), 'w'
|
||||
) as f:
|
||||
f.write(nameLine)
|
||||
if startLine:
|
||||
f.write(startLine)
|
||||
for line in results[mac]:
|
||||
f.write(line)
|
||||
if endLine:
|
||||
f.write(endLine)
|
||||
|
||||
|
||||
def wrapper(args):
|
||||
if not os.path.isfile(args['filepath']):
|
||||
return
|
||||
extract_each(args['filepath'])
|
||||
|
||||
|
||||
def main():
|
||||
p = argparse.ArgumentParser(description='separate data from MAC addr')
|
||||
p.add_argument(
|
||||
'filepath',
|
||||
help="file path"
|
||||
)
|
||||
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())
|
||||
wrapper(args)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -1,268 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import os
|
||||
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
|
||||
|
||||
|
||||
class Point(object):
|
||||
def __init__(self, x, y):
|
||||
self.x = round(float(x), 6)
|
||||
self.y = round(float(y), 6)
|
||||
|
||||
|
||||
class Circle(object):
|
||||
def __init__(self, p, radius):
|
||||
if isinstance(p, Point):
|
||||
self.center = p
|
||||
elif isinstance(p, list):
|
||||
self.center = Point(p[0], p[1])
|
||||
self.radius = round(float(radius), 6)
|
||||
|
||||
|
||||
def get_distance(p1, p2):
|
||||
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)) 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])
|
||||
)
|
||||
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
|
||||
r1 = c1.radius
|
||||
r2 = c2.radius
|
||||
|
||||
d = get_distance(p1, p2)
|
||||
# if to far away, or self contained - can't be done
|
||||
if d >= (r1 + r2) or d <= math.fabs(r1 - r2):
|
||||
return None
|
||||
|
||||
a = (r1 * r1 - r2 * r2 + d * d) / (2 * d)
|
||||
h = math.sqrt(pow(r1, 2) - pow(a, 2))
|
||||
x0 = p1.x + a*(p2.x - p1.x)/d
|
||||
y0 = p1.y + a*(p2.y - p1.y)/d
|
||||
rx = -(p2.y - p1.y) * (h/d)
|
||||
ry = -(p2.x - p1.x) * (h / d)
|
||||
return [
|
||||
Point(x0 + rx, y0 - ry),
|
||||
Point(x0 - rx, y0 + ry)
|
||||
]
|
||||
|
||||
|
||||
def get_intersecting_points(circles):
|
||||
points = []
|
||||
num = len(circles)
|
||||
for i in range(num):
|
||||
j = i + 1
|
||||
for k in range(j, num):
|
||||
res = get_two_circles_intersecting_points(circles[i], circles[k])
|
||||
if res:
|
||||
points.extend(res)
|
||||
return points
|
||||
|
||||
|
||||
def is_contained_in_circles(point, circles):
|
||||
for i in range(len(circles)):
|
||||
if (
|
||||
get_distance(point, circles[i].center) > circles[i].radius
|
||||
):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def get_polygon_center(points):
|
||||
center = Point(float('nan'), float('nan'))
|
||||
xs = []
|
||||
ys = []
|
||||
num = len(points)
|
||||
if num is 0:
|
||||
return center
|
||||
for i in range(num):
|
||||
xs.append(points[i].x)
|
||||
ys.append(points[i].y)
|
||||
center.x = median(xs)
|
||||
center.y = median(ys)
|
||||
return center
|
||||
# ======= end =======
|
||||
|
||||
|
||||
def calcInnerPoints(circles, bounds):
|
||||
inner_points = []
|
||||
for p in get_intersecting_points(circles):
|
||||
# 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
|
||||
if bounds.get('x_max', None) is not None and bounds['x_max'] < p.x:
|
||||
continue
|
||||
if bounds.get('y_min', None) is not None and bounds['y_min'] > p.y:
|
||||
continue
|
||||
if bounds.get('y_max', None) is not None and bounds['y_max'] < p.y:
|
||||
continue
|
||||
inner_points.append(p)
|
||||
return inner_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
|
||||
}
|
||||
'''
|
||||
points = []
|
||||
circles = []
|
||||
for loc in mydict:
|
||||
tmp = loc.split(',')
|
||||
p = Point(tmp[0], tmp[1])
|
||||
points.append(p)
|
||||
if mydict[loc][1]:
|
||||
for r in arange(
|
||||
max(mydict[loc][0] - mydict[loc][1], 0.001),
|
||||
mydict[loc][0] + mydict[loc][1],
|
||||
1
|
||||
):
|
||||
circles.append(Circle(p, r))
|
||||
else:
|
||||
circles.append(Circle(p, mydict[loc][0]))
|
||||
inner_points = calcInnerPoints(circles, bounds)
|
||||
if verbose:
|
||||
print('* Inner points:')
|
||||
if len(inner_points) is 0:
|
||||
print('*** No inner points detected!!')
|
||||
for point in inner_points:
|
||||
print('*** ({0:.3f}, {1:.3f})'.format(point.x, point.y))
|
||||
center = get_polygon_center(inner_points)
|
||||
return (center.x, center.y)
|
||||
|
||||
|
||||
def deriveLocation(args, results):
|
||||
# TODO: currently assume 2D
|
||||
loc_distance = {}
|
||||
for mac in results:
|
||||
if mac not in args['config_entry']:
|
||||
continue
|
||||
loc = args['config_entry'][mac].get('location', None)
|
||||
if loc is None:
|
||||
continue
|
||||
loc_distance[loc] = results[mac]
|
||||
loc = trilateration2d(
|
||||
loc_distance,
|
||||
bounds=args.get('loc_bounds', None),
|
||||
verbose=args.get('verbose', False)
|
||||
)
|
||||
if args.get('outfp', False):
|
||||
with open("{0}_locs".format(args['outfp']), 'a') as f:
|
||||
f.write(
|
||||
"{0:.6f},{1:.4f},{2:.4f}\n"
|
||||
.format(time.time(), loc[0], loc[1])
|
||||
)
|
||||
return loc
|
||||
|
||||
|
||||
def plotLocation(loc):
|
||||
handler = None
|
||||
try:
|
||||
import matplotlib.pyplot as plt
|
||||
handler = plt.scatter(loc[0], loc[1])
|
||||
plt.pause(0.01)
|
||||
except Exception:
|
||||
pass
|
||||
return handler
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
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
|
||||
|
|
@ -1,335 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import re
|
||||
import os
|
||||
import time
|
||||
import json
|
||||
import argparse
|
||||
import subprocess
|
||||
|
||||
from numpy import median, sqrt
|
||||
from libLocalization import deriveLocation
|
||||
|
||||
|
||||
def which(program):
|
||||
'''
|
||||
check if a certain program exists
|
||||
'''
|
||||
def is_executable(fp):
|
||||
return os.path.isfile(fp) and os.access(fp, os.X_OK)
|
||||
fp, fn = os.path.split(program)
|
||||
if fp:
|
||||
if is_executable(program):
|
||||
return program
|
||||
else:
|
||||
for path in os.environ["PATH"].split(os.pathsep):
|
||||
path = path.strip('"')
|
||||
exec_file = os.path.join(path, program)
|
||||
if is_executable(exec_file):
|
||||
return exec_file
|
||||
return None
|
||||
|
||||
|
||||
class Measurement(object):
|
||||
def __init__(self, interface, ofp=None, cali=(1.0, 0.0)):
|
||||
self.outf = None
|
||||
self.interface = interface
|
||||
# default file path for config for iw ftm_request
|
||||
self.config_fp = '/tmp/config_entry'
|
||||
if ofp:
|
||||
try:
|
||||
self.outf = open(ofp, 'w')
|
||||
self.outf.write(
|
||||
'MAC,caliDist(cm),rawRTT(psec),rawRTTVar,rawDist(cm),' +
|
||||
'rawDistVar,rssi(dBm),time(sec)\n'
|
||||
)
|
||||
self.outf.write(
|
||||
"ff:ff:ff:ff:ff:ff,nan,nan,nan,nan,nan,nan,{0:.6f}\n"
|
||||
.format(time.time())
|
||||
)
|
||||
except Exception as e:
|
||||
print(str(e))
|
||||
self.regex = (
|
||||
r"Target: (([0-9a-f]{2}:*){6}), " +
|
||||
r"status: ([0-9]), rtt: ([0-9\-]+) \(±([0-9\-]+)\) psec, " +
|
||||
r"distance: ([0-9\-]+) \(±([0-9\-]+)\) cm, rssi: ([0-9\-]+) dBm"
|
||||
)
|
||||
self.cali = cali
|
||||
if not self.check_iw_validity():
|
||||
exit(127) # command not found
|
||||
|
||||
def check_iw_validity(self):
|
||||
'''
|
||||
check if iw exists and support FTM commands
|
||||
'''
|
||||
iwPath = which('iw')
|
||||
if iwPath is None:
|
||||
print('Err: iw command not found!')
|
||||
return False
|
||||
p = subprocess.Popen(
|
||||
"iw --help | grep FTM",
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.PIPE,
|
||||
shell=True
|
||||
)
|
||||
out, err = p.communicate()
|
||||
if err:
|
||||
print('Err: {0}'.format(err))
|
||||
return False
|
||||
if 'FTM' not in out:
|
||||
print('Err: iw command does not support FTM')
|
||||
return False
|
||||
return True
|
||||
|
||||
def prepare_config_file(self, targets):
|
||||
if not isinstance(targets, dict):
|
||||
return False
|
||||
with open(self.config_fp, 'w') as of:
|
||||
for bssid in targets:
|
||||
of.write(
|
||||
"{0} bw={1} cf={2} retries={3} asap spb={4}\n"
|
||||
.format(
|
||||
bssid,
|
||||
targets[bssid]['bw'],
|
||||
targets[bssid]['cf'],
|
||||
targets[bssid]['retries'],
|
||||
targets[bssid]['spb'],
|
||||
)
|
||||
)
|
||||
return True
|
||||
|
||||
def get_distance_once(self, verbose=False):
|
||||
p = subprocess.Popen(
|
||||
"iw wlp58s0 measurement ftm_request " +
|
||||
"{0}".format(self.config_fp),
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.PIPE,
|
||||
shell=True
|
||||
)
|
||||
out, err = p.communicate()
|
||||
if err:
|
||||
print(err)
|
||||
exit(13)
|
||||
matches = re.finditer(self.regex, out)
|
||||
if not matches:
|
||||
return []
|
||||
result = []
|
||||
mytime = time.time()
|
||||
for match in matches:
|
||||
mac = match.group(1)
|
||||
status = int(match.group(3))
|
||||
rtt = int(match.group(4))
|
||||
rtt_var = int(match.group(5))
|
||||
raw_distance = int(match.group(6))
|
||||
raw_distance_var = int(match.group(7))
|
||||
rssi = int(match.group(8))
|
||||
if status is not 0 or raw_distance < -1000 or raw_distance > 10000:
|
||||
continue
|
||||
distance = self.cali[0] * raw_distance + self.cali[1]
|
||||
result.append(
|
||||
(mac, distance, rtt, rtt_var,
|
||||
raw_distance, raw_distance_var, rssi)
|
||||
)
|
||||
if verbose:
|
||||
print(
|
||||
'*** {0} - {1}dBm - {2} (±{3:.2f}cm)'
|
||||
.format(mac, rssi, raw_distance, sqrt(raw_distance_var))
|
||||
)
|
||||
if self.outf is not None:
|
||||
self.outf.write(
|
||||
"{0},{1:.2f},{2},{3},{4},{5},{6},{7:.6f}\n"
|
||||
.format(
|
||||
mac, distance, rtt, rtt_var,
|
||||
raw_distance, raw_distance_var,
|
||||
rssi, mytime
|
||||
)
|
||||
)
|
||||
return result
|
||||
|
||||
def get_distance_median(self, rounds=1, verbose=False):
|
||||
'''
|
||||
use median instead of mean for less bias with small number of rounds
|
||||
'''
|
||||
result = {}
|
||||
median_result = {}
|
||||
if rounds < 1:
|
||||
rounds = 1
|
||||
for i in range(rounds):
|
||||
# no guarantee that all rounds are successful
|
||||
for each in self.get_distance_once(verbose=verbose):
|
||||
if each[0] not in result:
|
||||
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(
|
||||
[sqrt(x[4]) * self.cali[0] for x in result[mac]]
|
||||
)
|
||||
)
|
||||
return median_result
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_value, traceback):
|
||||
# properly close the file when destroying the object
|
||||
if self.outf is not None:
|
||||
self.outf.write(
|
||||
"ff:ff:ff:ff:ff:ff,nan,nan,nan,nan,nan,nan,{0:.6f}\n"
|
||||
.format(time.time())
|
||||
)
|
||||
self.outf.close()
|
||||
|
||||
|
||||
def wrapper(args):
|
||||
if os.path.isfile(args['json']):
|
||||
args['config_entry'] = json.load(open(args['json'], 'r'))
|
||||
print('Successfully loaded {0}!'.format(args['json']))
|
||||
else: # default config
|
||||
args['config_entry'] = {
|
||||
'34:f6:4b:5e:69:1f': {
|
||||
'bw': 20,
|
||||
'cf': 2462,
|
||||
'spb': 255,
|
||||
'retries': 3
|
||||
}
|
||||
}
|
||||
counter = 1
|
||||
if args['plot']:
|
||||
try:
|
||||
import matplotlib.pyplot as plt
|
||||
from libLocalization import plotLocation
|
||||
handler = None
|
||||
fig = plt.figure()
|
||||
plt.ion()
|
||||
plt.xlim([-200, 500])
|
||||
plt.ylim([-10, 1000])
|
||||
except Exception:
|
||||
args['plot'] = False
|
||||
print('Cannot plot because lacking matplotlib!')
|
||||
with Measurement(
|
||||
args['interface'],
|
||||
ofp=args['outfp'], cali=args['cali']
|
||||
) as m:
|
||||
while 1:
|
||||
print('Round {0}'.format(counter))
|
||||
try:
|
||||
m.prepare_config_file(args['config_entry'])
|
||||
# only print out results
|
||||
results = m.get_distance_median(
|
||||
rounds=args['rounds'], verbose=args['verbose']
|
||||
)
|
||||
for mac in results:
|
||||
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)
|
||||
print(
|
||||
'* Derived location: ({0:.3f}, {1:.3f})'
|
||||
.format(loc[0], loc[1])
|
||||
)
|
||||
if args['plot']:
|
||||
try:
|
||||
if handler is not None:
|
||||
handler.remove()
|
||||
handler = plotLocation(loc)
|
||||
if handler is None:
|
||||
plt.close(fig)
|
||||
except KeyboardInterrupt:
|
||||
plt.close(fig)
|
||||
break
|
||||
except Exception:
|
||||
raise
|
||||
except KeyboardInterrupt:
|
||||
break
|
||||
except Exception as e:
|
||||
err = str(e)
|
||||
print(err)
|
||||
if 'Busy' in err:
|
||||
break
|
||||
time.sleep(10)
|
||||
counter += 1
|
||||
|
||||
|
||||
def main():
|
||||
p = argparse.ArgumentParser(description='iw measurement tool')
|
||||
p.add_argument(
|
||||
'--cali',
|
||||
nargs=2,
|
||||
# default=(0.9376, 558.0551), # indoor
|
||||
default=(0.8927, 553.3157), # outdoor
|
||||
type=float,
|
||||
help="calibrate calibration params (pre-defined outdoor by default)"
|
||||
)
|
||||
p.add_argument(
|
||||
'--outfp', '-f',
|
||||
default=None,
|
||||
help="if set, will write raw fetched data to file"
|
||||
)
|
||||
p.add_argument(
|
||||
'--rounds',
|
||||
default=1,
|
||||
type=int,
|
||||
help="how many rounds to run one command; default is 1"
|
||||
)
|
||||
p.add_argument(
|
||||
'--interface', '-i',
|
||||
default='wlp58s0',
|
||||
help="set the wireless interface"
|
||||
)
|
||||
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"
|
||||
)
|
||||
p.add_argument(
|
||||
'--indoor',
|
||||
default=False,
|
||||
action="store_true",
|
||||
help=(
|
||||
"if set, use default indoor calibration params " +
|
||||
"(will be ignored if `cali` is being used)"
|
||||
)
|
||||
)
|
||||
p.add_argument(
|
||||
'--locs',
|
||||
default=False,
|
||||
action="store_true",
|
||||
help="if set, derive location and store it to file"
|
||||
)
|
||||
p.add_argument(
|
||||
'--plot',
|
||||
default=False,
|
||||
action="store_true",
|
||||
help="if set, will plot the derived location in realtime"
|
||||
)
|
||||
try:
|
||||
args = vars(p.parse_args())
|
||||
except Exception as e:
|
||||
print(str(e))
|
||||
sys.exit()
|
||||
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['outfp']:
|
||||
fp, ext = os.path.splitext(args['outfp'])
|
||||
args['outfp'] = "{0}_{1}{2}".format(fp, args['time_of_exec'], ext)
|
||||
wrapper(args)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -1,128 +0,0 @@
|
|||
#!/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