resample.m

文件夹中NPFMain.m为滤波算法主运行程序

%resample.m
%函数功能：实现粒子滤波算法中的重采样算法
%特点：个人觉得此函数写得有些笨，应加以改进
function [order,result]=resample(S,w)

lengthOfS=length(S(1,:));
[A,I]=sort(S(1,:));%将采样值按大小排序
F=zeros(1,lengthOfS);
w_con=zeros(1,lengthOfS);
for k=1:lengthOfS
    w_con(k)=w(I(k));%求出序列A的概率分布函数
    S_con2(k)=S(2,I(k));
    S_con3(k)=S(3,I(k));
    S_con4(k)=S(4,I(k));
    S_con5(k)=S(5,I(k));
    F(k)=sum(w_con(1:k));
end


result=zeros(5,lengthOfS);
seed=rand(1,lengthOfS);
for seedNum=1:lengthOfS
    for samNum=1:lengthOfS
        if samNum==1
            if seed(seedNum)>=0 && seed(seedNum)<=F(1)
                result(1,seedNum)=A(samNum);
                result(2,seedNum)=S_con2(samNum);
                result(3,seedNum)=S_con3(samNum);
                result(4,seedNum)=S_con4(samNum);
                result(5,seedNum)=S_con5(samNum);
                order(seedNum)=I(samNum);
            end
        else
            if seed(seedNum)>F(samNum-1) && seed(seedNum)<=F(samNum)
                result(1,seedNum)=A(samNum);
                result(2,seedNum)=S_con2(samNum);
                result(3,seedNum)=S_con3(samNum);
                result(4,seedNum)=S_con4(samNum);
                result(5,seedNum)=S_con5(samNum);
                order(seedNum)=I(samNum);
            end
        end
        
    end
    
end