pf_update.m

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

function pf1 = pf_update(pf, xlate, func, varargin)

% x.s	state (x,y)
% x.pi	probability
% x.c	cumulative probability
	verbose = 0;

	N = pf.n; % number of particles
	x = pf.x; % cumulative probs of the N particles

	%xcheck(x)
	tic
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% resample the particles
	if verbose, disp('resample'); end
	c = [x(:).c];
	rn = rand(N,1) * 0.98; % pick N random numbers between 0 and 0.98
	for i=1:N,
		k = find( c >= rn(i));	% find all with cum prob greater than guess
		if ~isempty(k),
			k = k(1);
			xn(i).s = x(k).s;
        else
		       fprintf('rn(i) = %f\n', rn(i));
               xn(i).s=x(i).s; % I added this so it won't crash -- dk

		end
	end
	%xcheck(xn)

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% drift/diffuse
	if verbose, disp('drift/diffuse'); end
	for i=1:N,
		% drift
		if ~isempty(xlate)
			xn(i).s = xn(i).s + xlate;
		end

		% diffuse
		xn(i).s = xn(i).s + pf.diffuse * randn(2,1);

		% clip to keep within bounding values
		xn(i).s(1) = max( min(xn(i).s(1), pf.xmax), pf.xmin);
		xn(i).s(2) = max( min(xn(i).s(2), pf.ymax), pf.ymin);

	end
	%xcheck(xn)

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% measure
	if verbose, disp('measure'); end

    % Compute the probability of each particle ('prob' is a vector of
    % probabilities):
	prob = feval(func, [xn.s]', varargin{:});

	tot = 0;
	for i=1:N,
		xn(i).pi = prob(i);
		% probability is inversely proportional to range error
		% with an enforced minimum (floor)
		tot = tot + xn(i).pi;
	end
    
    if tot==0
        keyboard
    end
	%xcheck(xn)

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	if verbose, disp('normalize'); end
	for i=1:N,
		xn(i).pi = xn(i).pi/tot; % normalize each prob. by the sum of the probs.

        % Accumulate the cumulative probabilities:
		if i > 1,
			xn(i).c = xn(i).pi + xn(i-1).c;
		else
			xn(i).c = 0;
		end
	end

	%xcheck(xn)

	pf.dt = toc; % time to advance one step
	pf1 = pf;
	pf1.x = xn;