rs_regularized.m

particle filter 粒子滤波器 matlab工具箱

function [x,w] = rs_regularized(x,w,N,rs)
%RS_RESIDUAL residual resampling
%
% [X,W] = RS_RESIDUAL(X,W) resamples the states X
% and their associated weights W using residual
% resampling.
%
% [X,W] = RS_RESIDUAL(X,W,N) returns a resampled
% distribution of size N.
%
% [X,W] = RS_RESIDUAL(X,W,N,RS) uses RS as resample
% function to sample the remaining points from
% (defaults to RS_STRATIFIED).

% Copyright (C) 2004  Jeroen Hol
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

% check inputs
if nargin<3
    rs = @rs_stratified;
    N = length(w);
elseif nargin<4
    rs = @rs_stratified;
end

% calculate the number of copies
wa = N*w;
nk = floor(wa);
Nk = cumsum(nk);
K = Nk(end);

% indices of the particles to copy
ind = zeros(1,K);
k=1;
for i = 1:K
    while (Nk(k)<i)
        k = k + 1;
    end
    ind(i) = k;
end

% calculate the number of additonal copies
% and the weights for random resampling
wb = wa-nk;
wb = wb / sum(wb);
n = N-K;

% create new states and corresponding weights
if n>0
    x = [x(:,ind),feval(rs,x,wb,n)];
else
    x=x(:,ind);
end
w=ones(1,N)./N;