gusucode.com > 扩展卡尔曼滤波，粒子滤波，去偏卡尔曼滤波和循环增益尔曼滤波的源程序 > 粒子滤波/particle_filter.m

    function [filter_data]=data_kalman_filter(view_data,point_view_data,N,object_init_state,Q1,point_Q2,T,l,real_data)
%定义初试滤波
filter_data0=object_init_state';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ch=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];               %定义状态转移矩阵
M=10;                                               %定义粒子数为10
M_out=10;
%h=[1 0 0 0;0 0 1 0];                               %观测转移矩阵
%观测误差是随时间变化的所以需计算观测误差
%cos_2_v_thita=(object_init_state(1,1)^2)/(object_init_state(1,1)^2+object_init_state(1,3)^2);
%sin_2_v_thita=1-cos_2_v_thita;
%R_2=(object_init_state(1,1)^2+object_init_state(1,3)^2);
%Q2=zeros(2,2);       
%Q2(1,1)=point_Q2(1,1)*cos_2_v_thita+R_2*sin_2_v_thita*point_Q2(2,2);
%Q2(1,2)=sqrt(cos_2_v_thita)*sqrt(sin_2_v_thita)*(point_Q2(1,1)-R_2*point_Q2(2,2));
%Q2(1,2)=0;
%Q2(2,1)=Q2(1,2);
%Q2(2,2)=point_Q2(1,1)*sin_2_v_thita+R_2*cos_2_v_thita*point_Q2(2,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
for i=1:M
    w1=sqrt(Q1(1,1))*randn;
    w2=sqrt(Q1(2,2))*randn;
    w=[w1;w2];
    x_part(:,i)=filter_data0+l*w;
end
 q0=ones(1,M)/M;   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%根据初始预测计算滤波值
%通过输入观测值进行卡尔曼滤波
for i=1:N
    for k=1:M
        w1=sqrt(Q1(1,1))*randn;
        w2=sqrt(Q1(2,2))*randn;
        w=[w1;w2];
        xpart_minus(:,k)=ch*x_part(:,k)+l*w;
        r=sqrt(xpart_minus(1,k)^2+xpart_minus(3,k)^2);
        if xpart_minus(1,k)>0 
           thita=atan(xpart_minus(3,k)/xpart_minus(1,k));
       else
           thita=atan(xpart_minus(3,k)/xpart_minus(1,k))+pi;
       end
        z_part=[r;thita];
        vhat=point_view_data(:,i)-z_part;                               %观测值和预测值的差
        if i==1
           q(i,k)=1/(2*pi*sqrt(point_Q2(1,1)*point_Q2(2,2)))*exp(-(vhat(1)^2/(2*point_Q2(1,1))+vhat(2)^2/(2*point_Q2(2,2))))*q0(k);  
       else
           q(i,k)=1/(2*pi*sqrt(point_Q2(1,1)*point_Q2(2,2)))*exp(-(vhat(1)^2/(2*point_Q2(1,1))+vhat(2)^2/(2*point_Q2(2,2))))*q(i-1,k); 
       end
    end
    qsum=sum(q(i,:));
    for k=1:M
        q(i,k)=q(i,k)/qsum;                                 %  归一化权值
    end
    %x_part=xpart_minus;
    qsum2=q(i,:)*q(i,:)';
    Nff=1/qsum2;
   
    if Nff<2/3*M
        u=rand/M_out; 
       
        for k=1:M           
            qtempsum=0;
            for j=1:M
                qtempsum=qtempsum+q(i,j);
                if qtempsum>= u
                   x_part(:,k) = xpart_minus(:,j);
                   u=u+1/M_out;
                   break; 
                end
           end
        end
        q(i,:)=ones(1,M)/M;
    else
        x_part=xpart_minus;
    end
    %重采样有问题
 
   x_filt=zeros(4,1);
   for k=1:M
       x_filt=q(k)*x_part(:,k)+x_filt;
   end
   filter_data(:,i)=x_filt;
 
    
end