www.pudn.com > pfdemo.rar > pfdemo.m, change:2006-09-25,size:2339b


%Matlab code that implements the simulation and the particle filter 
% Particle filter for homework 2 of Assignment 8 
%%%说明:断点设置在63行可以看出k不同值时的粒子权值更新情况 
clear all; 
M=100; %number of particle  
eps=0.00; 
c=3.8; 
R=[0.005]; 
sqrtR=sqrtm(R); 
 
endK=5; 
 
%Genetate state and observation sequences 
 
XTrue(:,1)=rand(1,1);%initial state 
for k=1:endK 
    XTrue(:,k+1)=c*XTrue(:,k)*(1-XTrue(:,k));%%the state equation 
    Z(:,k)=cos(pi*XTrue(:,k+1))+sqrtR*rand(1,1);%%the observation equation 
end 
 
%Plot the true state and obserbations 
 
figure(1); 
clf; 
subplot(2,1,1); 
plot(0:endK,XTrue(1,:),'b*',0:endK,XTrue(1,:)); 
title('True Values'); 
subplot(2,1,2); 
plot(1:endK,Z(1,:)); 
title('Observations'); 
 
%Generate the initial particles 
X=rand(1,M); 
w=1/M*ones(1,M); 
xhat(:,1)=X*w'; 
 
%plot the initial particles 
figure(2); 
clf; 
stem(X(1,:),w(1,:)); 
title('Initial Particles'); 
 
%Do the particle filter 
 
for k=1:endK  %%% k time 
    %Extend the particles one time step 
    %This is sampling from the proposal distribution p(x_{k+1}|x_{k}) 
    xExt=c*X(1,:).*(1-X(1,:))+2*eps*rand(1,M)-eps; 
    %Compute the weights 
    w=exp(-(Z(1,k)-cos(pi*xExt)).^2/(2*R)).*w; 
    w=w/sum(w); 
     
    %Compute the estimate of the state 
    xhat(:,k+1)=xExt*w'; 
     
    %plot the reweighted particles 
    figure(3) 
    clf; 
    stem(xExt(1,:),w(1,:)); 
    title ('Update Particles and weights'); 
     
    %Resampling the particle  
    len=length(w); 
     
    %Cumulative Distributive Function 
    %                                                                                                                                                                               
    cumpr=cumsum(w(1,1:len))'; 
    cumpr=cumpr/max(cumpr); 
     
    u(1,1)=(1/M)*rand(1,1); 
    i=1; 
    for j=1:M 
        u(j,1)=u(1,1)+(1/M)*(j-1); 
        while(u(j,1)>cumpr(i,1)) 
            i=i+1; 
            if i>M 
                break 
            end 
        end 
        if i<=M 
            x_update(:,j)=xExt(:,i); 
        end 
    end 
    X=x_update; 
    w=1/M*ones(1,M); 
end 
 
%Plot estimated data 
figure(4); 
clf; 
plot(0:endK,XTrue(1,:),'b--',0:endK,xhat(1,:),'r-'); 
title('Estimated state sequence'); 
legend('True','Estimated')