updatestates.m

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

function [xPost,qUpdate] = updatestates(xPre,q);     % 00/070129
% 功能   ： 进行SIR算法的重采样过程，其中进行了是否进行重采样的判断
% 输入   ： - xPre = 状态预测值
%           - q = 归一化权值
% 输出   ： - xPost = 重采样后的状态采样（粒子）值
%           - qUpdate = 更新后的权值

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

% 重采样
% ======

[N,timeStep] = size(xPre);
qNzero = length(find(q > 0.0001));      % 得到权值非零粒子数目
x = ones(size(xPre));

i = 1;
r = 0.9;
% 当非零粒子数目小于r*粒子数目时，进行重采样，同时更新权值
% 否则不进行重采样，权值不变
if qNzero < N * r 
    xPost = resampling(xPre,q);
    qUpdate = 1/N * ones(N,1);
else
    xPost = xPre;
    qUpdate = q;
end;