mypf.m

％ ％ set some variables in the workspace to control behaviour: ％ ％ graphicsMode 0 no graphics, ％

%
% set some variables in the workspace to control behaviour:
%
%	graphicsMode	0 no graphics,
%			1 graphics, particles, beacons, ground truth
%			2 graphics, as above + range circles
%
%	perfect		0 use beacon range data
%			1 use ground truth range data
% clear all

addpath ./peterlib_web

AVI_TAG = 0;
INTERP=0;
begin_run = 200;

if ~exist('graphicsMode'),
	graphicsMode = 2;
end
rand_state_prev = randn('state');
%randn('state', rand_state);

% set perfect=1 to localize using ground truth data rather than 
%  range measurements
if ~exist('perfect')
    perfect = 0;
end

if AVI_TAG == 1
    mov = avifile('PF_movie.avi');
    mov.fps = 2;
    mov.Compression = 'Cinepak';
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% parameters
axdata = [-20 20 -15 25];

N = 1000;	% number of particles
B = 0.03;		% diffusion rate (m per second)
L = 80;	% number of points along path

feet2m = 12 * 0.0254;
rmin = 10 * feet2m;
rmax = 70 * feet2m;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

pf = pf_init(axdata, N,'robot');
pf.diffuse = B;

%load circle 
load fig8

GTX=GTX(begin_run:end,:);
DRX=DRX(begin_run:end,:);
TDX=TDX(begin_run:end,:);
TDC=TDC(begin_run:end,:);
TDP=TDP(begin_run:end,:);

if L>length(GTX), L=length(GTX-1), end

% TL = tag locations = [tag_ID  x_coordinate  y_coordinate]
%              units = [ ---       meters        meters   ]
beacon = TL(:,2:3);

% TD = tag data = [timestamp  antenna  tag_ID  range]
%         units = [ seconds     ---     ---      ft ]
%        (tag_ID is used to look up the lag location in TL)

% DR_integrated = integrated dead reckoning =
%                       [timestamp  x_coordinate  y_coordinate  heading]
%                units = [ seconds      meters        meters radians]
%
% DRX = extracted dead reckoning.  These are just the points from
%  DR_integrated at which a range measurement was received from an RF tag.

if perfect,
	TD = TDP;
else
    TD=TDC;
end


% convert tag IDs to beacon indexes, make life easier later.
for i=1:numrows(TL),
	id = TL(i,1);
	TD( TD(:,3)==id,3 ) = i;
end

rangeIndex = 1;

xmin = min(beacon(:,1));
xmax = max(beacon(:,1));
ymin = min(beacon(:,2));
ymax = max(beacon(:,2));


zz = [];

[xy_est,xy_var] = pf_stats(pf);  % [xy_est,xy_var] = [Mean, Var]
% xy_est = the weighted mean of all the particles

reject = 0;
for j=1:L,

    % get range from active beacon to target
	rec = TD(rangeIndex,:);
	rangeIndex = rangeIndex + 1;

	id = rec(3);
	bpos = beacon(id, :)';
    
    % replace meas. with mean from PDF
    if perfect==0
        rangeIndex2 = find(PDF(:,1)>=rec(4));
        rangeIndex2 = min(rangeIndex2);
        if size(rangeIndex2,1)==0
            rangeIndex2 = find(PDF(:,1)<=rec(4));
            rangeIndex2 = max(rangeIndex2);
        end
        range = PDF(rangeIndex2,2)*feet2m; % mean
        sig = sqrt(PDF(rangeIndex2,3))*feet2m; % standard deviation
    else
        range=rec(4)*feet2m+randn(1);
        sig = 1.47;
    end
    
	ts = rec(1); % timestamp
	if ~exist('ts_p')
		ts_p = ts;
	end
		
	if j == 1,
		ts0 = ts;
	end

	rerr = norm(xy_est-bpos') - range;
	confidence = abs(rerr/sqrt(xy_var));
	fprintf('step %3d, rerr = %6.2f, std = %4.2f, rerr/std = %4.2f\n', ...
		j, rerr, sqrt(xy_var), confidence );

    if INTERP == 1
	% iterpolate robot's position from deadreckoning
    dr=interp1(DRX(:,1), DRX(:,2:3), ts);
    else
        dr=DRX(j,2:3);
    end
    
    % compute difference in dead reckoning, use this to move
	% the particles
	if j == 1,
		dr_p = dr;
	end
	ddr = dr - dr_p;
	dr_p = dr;

	pf.diffuse = B * (ts - ts_p);
	ts_p = ts;

		pf = pf_update(pf, ddr', @range_func, bpos, range, sig);

	if graphicsMode > 0,
		h = pf_plot(pf);
	end

	[xy_est,xy_var] = pf_stats(pf);


	if graphicsMode > 0,
		axes(h)
		hold on
		plot(beacon(:,1), beacon(:,2), 'bd');
		plot(beacon(id,1), beacon(id,2), 'r*');
		if graphicsMode > 1,
			circle(bpos, range, 'r-');
        end
		plot(GTX(1:1*L,2), GTX(1:1*L,3), 'b'); % ??? Looks right, so why does plotting 
        plot(xy_est(1), xy_est(2), 'mo');  % continue past end of this blue path in anim.?  Because of interpolation.
	end

    % interpolate robot position from groundtruth at beacon timestamp
    if INTERP == 1
    	x_gt = interp1(GTX(:,1), GTX(:,2), ts);
    	y_gt = interp1(GTX(:,1), GTX(:,3), ts);
    	th_gt = interp1(GTX(:,1), GTX(:,4), ts);
    else
    	x_gt = GTX(j,2);
	    y_gt = GTX(j,3);
    	th_gt = GTX(j,4);
    end

	if graphicsMode > 0,
		plot(x_gt, y_gt, 'g+');
		hold off

		drawnow
        if AVI_TAG == 1
            F = getframe(gca);
            mov = addframe(mov,F);
        end
	end

	% 1	t
	% 2,3	x,y est
	% 4	x+y std
	% 5,6	x,y dead reckoning
	% 7,8	x,y ground truth
	% 9	    0 -- used to be rms error
	% 10	range error
	% 11	confidence in measurement
	% 12	beacon id
	% 13	theta ground truth
% 	zz = [zz; ts-ts0 xy_est sqrt(xy_var) dr x_gt y_gt norm22(xy_est(1)-x_gt, xy_est(2)-y_gt) rerr confidence id, th_gt];
	zz = [zz; ts-ts0 xy_est sqrt(xy_var) dr x_gt y_gt  0 rerr confidence id  th_gt];
end

if AVI_TAG == 1
    mov = close(mov);
end