simmle.m

无线传感器节点定位算法

tic
global refDevices blindDevices totalDevices linearRefLocs dhat funcEvals dfuncEvals;
load patwari03meas_sIV_v2.mat

% Basic simulation parameters
roomSize        = [10,10];       % Room size, meters
refDevices      = 4;           % How many references 
blindDevices    = 40;
trials          = 3;          % How many indep trials to run
measMethod      = 'R';         % Use 'R' for RSS
totalDevices    = 44;
blindCoords     = 2*blindDevices;
actualRefLocs   = [-3.2004,11.43; -2.7432,1.6002; 9.6012,10.516; 9.6012,0];
linearRefLocs   = [actualRefLocs(:,1)', actualRefLocs(:,2)'];

% Optimization parameters
ftol  = 0.00001;

func  = 'calcError';      
dfunc = 'calcDError';      

%for i=1:44
%    xi=deviceLocs([i],[1]);
%    yi=deviceLocs([i],[2]);
%    plot(xi,yi,'.',10,5);
%    hold on
%end
%grid

%| 1. Set up the blindfolded device locations
%xBlind   = [5 7.5 5 2.5 3];% 4 6 3 8 2.5 1 3 2 9 3.2 2.5 7.5 8 1 6 5.5 8 6 9 5 1.5];
%yBlind   = [7.5 4.5 5 2.5 2];% 2 1 9 5 5 6 7 4 1.5 5.5 3 8 7.2 1 4 2.7 3 8 9 8.5 8];
xBlind   = [deviceLocs(1:2,1)' deviceLocs(4:9,1)' deviceLocs(11:34,1)' deviceLocs(36:43,1)'];
yBlind   = [deviceLocs(1:2,2)' deviceLocs(4:9,2)' deviceLocs(11:34,2)' deviceLocs(36:43,2)'];
actualBlindLocs = [xBlind', yBlind'];
actualAllLocs   = [actualRefLocs; actualBlindLocs];
xActual         = actualAllLocs(:,1)';
yActual         = actualAllLocs(:,2)';
actualDist      = L2_distance(actualAllLocs', actualAllLocs',0);

%| 2.  Define the channel model
    sigmaOverN = 1.7;                            
    % If C==1, then this simulation runs the _true_ MLE.
    % If C==exp( 0.5* (log(10)/10 *sigmaOverN)^2), then this runs a
    %   bias-corrected (pseudo-) MLE.
    C = exp( 0.5* (log(10)/10 *sigmaOverN)^2);   
    %C = 1;

for trial = 1:trials,
    
    
        %| 3.0 Generate a random set of RSS-based distance measurements.  
        dhat  = actualDist.*10.^(sigmaOverN/10 .*symrandn(totalDevices))./C;
        %dhat  = [tilde_d_RSS];
        %dhat  = actualDist.*10.^(sigmaOverN/10 .*symrandn(totalDevices));

    %| 4.  Make an initial guess of the coordinates.
    blindLocs0 = [xBlind, yBlind]; % true coordinates
    
    %| 5.  Find optimum locations
    funcEvals = 0;  dfuncEvals = 0;
    [coordsMLE, iter, errorMin] = frprmn(blindLocs0, ftol, func, dfunc, 0);
    disp(sprintf('%d: Function / Deriv. evals: %d / %d.', trial, funcEvals, dfuncEvals));
    
    %| 6.  Save the resulting estimated coords
    coordEsts(trial, 1:blindCoords) = coordsMLE;
    errorEsts(trial, 1:blindCoords) = errorMin;
end % for trial

estMean = mean(coordEsts);
errorestMean = mean(errorEsts);
estCov  = cov(coordEsts);
estVars = diag(estCov);
estStds = sqrt(estVars);
locVars = estVars(1:blindDevices) + estVars((blindDevices+1):(2*blindDevices));
locStd  = sqrt(locVars);
 
toc  % show time of execution

figure; clf;

for i=1:blindDevices,
    hold on
    R = cov(coordEsts(:,i), coordEsts(:,blindDevices+i));
    drawOval(estMean(i), estMean(blindDevices+i), R, 'b-','v', 8, 0, 1);
    plot(estMean(i), estMean(blindDevices+i),'bv','markersize',10);
    plot(xBlind(i), yBlind(i),'r.','markersize',20);
    f_2Dwfenum(deviceLocs,'g',0.1);
    hold  on

end
hold on
plot(actualRefLocs(:,1)', actualRefLocs(:,2)','g.','markersize',30);
set(gca,'xlim',[-6 11])
set(gca,'ylim',[-2 14])
set(gca,'FontSize',18)
set(gca,'DataAspectRatio',[10 10 10])
xlabel('X Position (m)')
ylabel('Y Position (m)')
set(gca,'xTick',-5:2.5:11)
set(gca,'yTick',-2:2.5:14)
grid;

% Use for comparison
RMS_est_Std = sqrt(mean(locStd.^2))