my_updatestates3.m

用matlab程序编写的一个三维雷达跟踪粒子滤波器

function x = my_updatestates3(xu_sample,q);
% PURPOSE : Performs the resampling step of the SIR algorithm
%           in order(numSamples) steps.
% INPUTS  : - xu = Predicted state samples.
%           - q = Normalised importance ratios.
% OUTPUTS : - x = Resampled state samples.  5-500

%   how to invoke this function

% q(:,t+1) = importanceweights(xu_sample(:,t),y(t+1,1),R);
% x(:,t+1) = updatestates(xu_sample(:,t),q(:,t+1));

%xu_sample(:,:,t) =my_predictstates3(samples(:,:,t),Q);
%q(:,:,t+1) =my_importanceweights3(xu(:,:,t),y(:,t+1),R);    
%samples(:,:,t+1) =my_updatestates3(xu(:,:,t),q(:,:,t+1));      %perform  the resampling step

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

if nargin < 2, error('Not enough input arguments.'); end

[ss,N]=size(xu_sample);    % N = number of samples;
u = rand(1,1+N);
t = -log(u);
x = 10.*ones(size(xu_sample));
T = cumsum(t);
Q = cumsum(q);
i = 1;
j = 1;
while j <= N,
  if (Q(1,j)*T(1,N)) > T(1,i)
    x(:,i) = xu_sample(:,j); 
    i = i+1;
  else
    j = j+1;
  end;
end;