function Particle

% Particle filter 

x = 0.1; % 初始狀態
Q = 50; % 過程噪聲協方差
R = 50; % 測量噪聲協方差
tf1 = 100; % 仿真長度
tf = 150;

N =50; % 粒子濾波器粒子數

xhat = x;
P = 2;
xhatPart = x;

% 初始化粒子過濾器
for i = 1 : tf1
    xpart(i) = x + sqrt(P) * randn;
end

xArr = [x];
yArr = [sin(x) + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];

close all;

for k = 1 : tf1
    % 系統仿真
    x = 0.5 * x +  50 * sin((k-1)/15) + sqrt(Q) * randn;%狀態方程
    y = sin(x) + sqrt(R) * randn;%觀測方程
     %  卡爾曼濾波
    F = 0.5 ;
    P = F * P * F' + Q;
    H = sin(xhat) ;
    K = P * H' * inv(H * P * H' + R);
    xhat = 0.5 * xhat  + 50 * sin((k-1)/15);%預測
    xhat = xhat + K * (y - sin(xhat));%更新
    P = (1 - K * H) * P;
   
    for i = 1 : tf1
        xpartminus(i) = 0.5 * xpart(i) + 50 * sin((k-1)/15) + sqrt(Q) * randn;
        ypart = sin(xpartminus(i));
        vhat = y - ypart;%觀測和預測的差
        q(i) = (1 / (sqrt(R^2) * sqrt(2*pi))) * exp(-vhat^2 /( 2 * R^2));
    end
    %正常化的可能性，每個先驗估計
    qsum = sum(q);
    for i = 1 : tf1
        q(i) = q(i) / qsum;%歸一化權重
    end
    % 重采樣
    for i = 1 : tf1
        u = rand; % 均勻隨機數介于0和1
        qtempsum = 0;
        for j = 1 : tf1
            qtempsum = qtempsum + q(j);
            if qtempsum >= u
                xpart(i) = xpartminus(j);
                break;
            end
        end
    end
    xhatPart = mean(xpart);
    xArr = [xArr x];
    yArr = [yArr y];
    xhatArr = [xhatArr xhat];
    PArr = [PArr P];
    xhatPartArr = [xhatPartArr xhatPart];
    
    x0=27;
    xhat1 = x0;
    xhatPart1 = x0;  
    % 初始化粒子過濾器
for i = 1 : N
    xpart1(i) = x0 + sqrt(P) * randn;
end

xArr1 = [x0];
yArr1 = [x0 + sqrt(R) * randn];
xhatArr1 = [x0];
xhatPartArr1 = [xhatPart1];

close all;
    % 系統仿真
    x1 =  x0  + sqrt(Q) * randn;%狀態方程
    y1 = x1+ sqrt(R) * randn;%觀測方程
     %  卡爾曼濾波
    F1 = 1 ;
    P1 = F1 * P * F1' + Q;
    H1 = xhat1;
    K1 = P1* H1' * inv(H1 * P1 * H1' + R);
    xhat1 =  xhat1 ;%預測
    xhat1 = xhat1 + K1 * (y1 - xhat1);%更新
    P1 = (1 - K1 * H1) * P1;
   
    for i = 1 : N
        xpartminus1(i) =  xpart1(i)  + sqrt(Q) * randn;
        ypart1 = xpartminus1(i);
        vhat1 = y1 - ypart1;%觀測和預測的差
       q1(i) = (1 / (sqrt(R^2) * sqrt(2*pi))) * exp(-vhat1^2 /( 2 * R^2));
    end
    %正常化的可能性，每個先驗估計
    qsum = sum(q1);
    for i = 1 : N
        q1(i) = q1(i) / qsum;%歸一化權重
    end
    % 重采樣
    for i = 1 : N
        u = rand; % 均勻隨機數介于0和1
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + q(j);
            if qtempsum >= u
                xpart1(i) = xpartminus1(j);
                break;
            end
        end
    xhatPart1 = mean(xpart1);
    xArr1 = [xArr1 x1];
    yArr1 = [yArr1 y1];
    xhatArr1 = [xhatArr1 xhat1];
    PArr = [PArr P];
    xhatPartArr1 = [xhatPartArr1 xhatPart1];
    
t1 = 0 : tf1; 
t2=100:150;   
  end
end

figure;
plot(t1, xArr, 'b.', t2, xArr1, 'b.',t1,xhatArr,'r',t2,xhatArr1,'r',t1, xhatPartArr, 'x',t2,xhatPartArr1, 'x');
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('state');
legend('True state','True state', 'KF', 'KF', 'Particle filter estimate');

 
figure;
xhatRMS = sqrt((xArr - xhatArr).^2 / tf1);
xhatRMS1 = sqrt((xArr1 - xhatArr1).^2 / (tf-tf1));
subplot(2,1,1),plot(t1,xhatRMS);
set(gca,'FontSize',12); set(gcf,'Color','White');
title('正弦運動誤差值');xlabel('時間(S)'),ylabel('RMSE(米)');
subplot(2,1,2),plot(t2,xhatRMS1);
set(gca,'FontSize',12); set(gcf,'Color','White');
title('直線運動誤差值');xlabel('時間(S)'),ylabel('RMSE(米)');

figure;
xhatPartRMS= sqrt((xArr - xhatPartArr).^2 / tf1);
xhatPartRMS1 = sqrt((xArr1 - xhatPartArr1).^2 / (tf-tf1));
subplot(2,1,1),plot(t1,xhatPartRMS);
set(gca,'FontSize',12); set(gcf,'Color','White');
title('正弦運動誤差值');xlabel('時間(S)'),ylabel('RMSE(米)');
subplot(2,1,2),plot(t2,xhatPartRMS1);
set(gca,'FontSize',12); set(gcf,'Color','White');
title('直線運動誤差值');xlabel('時間(S)'),ylabel('RMSE(米)');