redistribution_uniform.m

matlab实现的粒子滤波器源代码

%================================================================================================================%
%
% Author: Tanya BERTOZZI
% Date:   24/03/03
%                                         
% Function for the redistribution of the particles
% 
% 
%================================================================================================================%

function[SupportOne,Weight,Weight_n] = redistribution_uniform(NPart,PrevSupportOne,PrevWeight)

%================================================================================================================%
% Calculation of the vector of the sum of weights
%================================================================================================================%

sumWeight(1) = PrevWeight(1);
for (indexWeight=2:NPart)
    sumWeight(indexWeight) = sumWeight(indexWeight-1) + PrevWeight(indexWeight);
end % end for (indexWeight=2:NPart)

%================================================================================================================%
% Redistribution of the particles
%================================================================================================================%

for (indexPart=1:NPart)
    
    %------------------------------------------------------------------------------------------------------------%
    % Generation of the uniform realization 
    %------------------------------------------------------------------------------------------------------------%
    
    unifRealiz = rand(1,1);
    
    %------------------------------------------------------------------------------------------------------------%
    % Determination of the index of the particle on which the particle indexPart must be redistributed 
    %------------------------------------------------------------------------------------------------------------%
    
    lowerBound = 1;
    upperBound = NPart;
    
    while ((upperBound-lowerBound~=1)&(sumWeight(indexPart)~=unifRealiz))
        meanValue    = (upperBound+lowerBound)/2;  
        redistrIndex = ceil(meanValue);
        if (sumWeight(redistrIndex)>=unifRealiz)
            upperBound = redistrIndex;
        else
            lowerBound = redistrIndex;
        end % end if (sumWeight(redistrIndex)>=unifRealiz)
    end % end while ((upperBound-lowerBound~=1)&(sumWeight(indexPart)~=unifRealiz)) 
    
    redistrIndex = upperBound;
  
    %------------------------------------------------------------------------------------------------------------%
    % Redistribution of the particle indexPart
    %------------------------------------------------------------------------------------------------------------%
    SupportOne(indexPart,1) = PrevSupportOne(redistrIndex,1);
     Weight(indexPart)       = 1/NPart;
end % end for (indexPart=1:NPart)   
SupportOne;
Weight=Weight';


%normalization of Weight
    Weight_n=Weight/sum(Weight);