%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')