resampling.m

采用误差校正算法改进的粒子滤波器

function x = resampling(xPre,q);
% 功能   ： 对当前采样集合进行重采样，预防粒子塌陷，缺点是引起粒子多样性损失
% 输入   ： - xPre = 状态采样预测值
%           - q = 归一化权值
% 输出   ： - x = 重采样后的采样集合



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

[N,timeStep] = size(xPre); 
u = sort(rand(N,1));
x = 5.*ones(size(xPre));
Q = cumsum(q);
i = 1;
j = 1;
% 依据重采样方法中最常用的随机采样方法
while  j <= N,                         
    if   i <= N & Q(j,1) > u(i,1),             % i <= N 预防下标溢出，两个条件不可颠倒
          x(i,1) = xPre(j,1);
          i = i+1;
    else
          j = j+1;
    end;
end;