function_bootstrap.m

粒子滤波程序

function[s_x_new]=Function_bootstrap(S,  s_x,    Z,  dsita);

    S_R=1;
    S_Q=1000;
    
    %=====predictstates=====
        s_w=sqrt(S_Q)*randn(S,6);
        s_xu=s_x+s_w;

    %=====importanceweights=====
        m=s_xu*dsita;
        s_q_sum=sum(    exp(    -0.5*S_R^(-1)*     (   [ones(S,1)*Z(1),ones(S,1)*Z(2)]-m   ).^2      )     );   
        for s=1:S,
            s_q(s,:)=exp(-0.5*S_R^(-1)*(Z'-m(s,:)).^2)./s_q_sum;
        end
    
    %=====updatestates=====
     u_u=rand(S+1,1);
     u_t=-log(u_u);
     s_x_new=s_xu;%10.*ones(S,1);
     u_T=cumsum(u_t);
     u_Q=cumsum(s_q(:,:));
     ii=1;jj=1;
     while jj<=S,
         if(u_Q(jj,1)*u_T(S))>u_T(ii)
             s_x_new(ii,1:3)=s_xu(jj,1:3);
             ii=ii+1;
         else
             jj=jj+1;
         end;
     end;  %while loop end
     ii=1;jj=1;
     while jj<=S,
         if(u_Q(jj,2)*u_T(S))>u_T(ii)
             s_x_new(ii,4:6)=s_xu(jj,4:6);
             ii=ii+1;
         else
             jj=jj+1;
         end;
     end;  %while loop end