x = 0; % 初始狀態(tài)
R = input('請輸入過程噪聲方差R的值: ');; % 測量噪聲協(xié)方差 
Q = input('請輸入觀測噪聲方差Q的值: '); % 過程狀態(tài)協(xié)方差
tf = 100; % 模擬長度
N = 100; % 粒子濾波器中的粒子個數(shù)
xhat = x;
P = 2;
xhatPart = x;
 
%粒子濾波器初期
for i = 1 : N
    xpart(i) = x + sqrt(P) * randn;
end
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];
close all;
 
for k = 1 : tf
    % 模擬系統(tǒng)
    x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
    y = x^2 / 20 + sqrt(R) * randn;
   % 粒子濾波器
    for i = 1 : N
        xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
        ypart = xpartminus(i)^2 / 20;
        vhat = y - ypart;% 觀測和預(yù)測的差
        q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
    end
    % 平均每一個估計的可能性
    qsum = sum(q);
    for i = 1 : N
        q(i) = q(i) / qsum;%歸一化權(quán)值
    end
   % 重采樣
    for i = 1 : N
        u = rand; 
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + q(j);
            if qtempsum >= u
                xpart(i) = xpartminus(j);
                break;
            end
        end
    end
   
    xhatPart = mean(xpart);
   
   % 在圖片中表示出數(shù)據(jù)
    xArr = [xArr x];
    yArr = [yArr y];
    xhatArr = [xhatArr xhat];
    PArr = [PArr P];
    xhatPartArr = [xhatPartArr xhatPart];
end
 
t = 0 : tf;
 
figure;
plot(t, xArr, 'b.', t, xhatPartArr, 'k-');
set(gca,'FontSize',10); set(gcf,'Color','yellow'); 
xlabel('time step'); ylabel('state');
legend('真實值', 'PF估計值');

xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
disp(['PF估計誤差均方值 = ', num2str(xhatPartRMS)]);