updatestates.m

对运动声目标进行航迹跟踪

function x = updatestates(thite,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.

% AUTHOR  : Nando de Freitas - Thanks for the acknowledgement :-)
% DATE    : 08-09-98

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

% 重采样  
xu=thite;
N=length(xu);
    P = cumsum(q);               % 计算q的累加值，维数和q一样


    ut(1)=rand(1)/N;
    k = 1; 
    i = zeros(1,N);
    for j = 1:N
        ut(j)=ut(1)+(j-1)/N;
        while(P(k)<ut(j));
        k = k + 1;
        end;
    i(j) = k;
    q(j)=1/N;
    end;
                         
    x = xu(:,i);                % The new particles
    
% xu=thite.';
% N=length(xu)  % N = number of samples;
% u = rand(N+1,1);
% t = -log(u);
% x = 10.*ones(size(xu));
% T = cumsum(t);
% Q = cumsum(q);
% i = 1;
% j = 1;
% while j <= N,
%   if (Q(j,1)*T(N,1)) > T(i,1)
%     x(i,1) = xu(j,1); 
%     i = i+1;
%   else
%     j = j+1;
%   end;
% end;