% 一維particle濾波ji 加EKF 加rms
% Process function:
%           x(k) = x(k-1)./2 + 25*x(k-1)./(1 + x(k-1).^2) + 8*cos(1.2*k-1) + w(k);
%
% Measurement function:
%           y(k) = (x(k)^2)/20 + v(k)
%
% Date: 3/31/2006

clear;
clc;
close all;
N = 1000;                 % Number of particles
P0 = 10;%;2;                   % Initial process noise covariance
Q = 10;                   % Process noise covariance
R = 1;                    % Measurement noise covariance
T=60;%50;                    % Step of time
m0=0;  
M=50;                     % numbers of experiment
for m=1:M

x0=0.1;                   % 系統(tǒng)初始值
xtrue(1)=x0; % Initial state value
y(1)=(xtrue(1)^2)/20+sqrt(R)*randn(1);

for k=2:T             % Simulate the system
    xtrue(k)=xtrue(k-1)/2+25*xtrue(k-1)/(1+xtrue(k-1)^2)+8*cos(1.2*(k-1))+sqrt(Q)*randn(1);% 計(jì)算真實(shí)值x
    y(k)=(xtrue(k)^2)/20+sqrt(R)*randn(1);    % 計(jì)算真實(shí)值y
end
xTrue=xtrue;   % 真實(shí)值





%EKF initial
xhat=m0;
xekf(1)=xhat;
Pest=P0;

x = sqrt(P0)*randn(1,N);          % Initialize the particles
xpf(1) = mean(x);
tic;
for k = 2:T
    x = x./2 + 25*x./(1 + x.^2) + 8*cos(1.2*(k-1))+sqrt(Q)*randn(1,N);
    h=(x.^2)/20;
    e = repmat(y(k),1,N) - h;  % Calculate weights
    q0 =(1/sqrt(2*pi*R))*exp(-e.^2/(2*R));             % The likelihood function
    q = q0/sum(q0);               % Normalize the importance weights
    
    % 重采樣                     
    P = cumsum(q);               % 計(jì)算q的累加值，維數(shù)和q一樣


    ut(1)=rand(1)/N;
    kk = 1; 
    i = zeros(1,N);
    for j = 1:N
        ut(j)=ut(1)+(j-1)/N;
        while(P(kk)<ut(j));
        kk = kk + 1;
        end;
    i(j) = kk;
    q(j)=1/N;
    end;
                         
    x = x(:,i);                % The new particles
    xpf(k) = mean(x);           % Compute the estimate
      
    % EKF filtering
    % predict
    f=0.5+25*(1-xhat^2)/((1+xhat^2)^2);
    xhat=xhat/2 + 25*xhat/(1 + xhat^2) + 8*cos(1.2*(k-1));
    Pest=f*Pest*f' + Q;
    % observe
    % 計(jì)算觀測(cè)方程一階導(dǎo)數(shù)
    h=xhat/10;
    ypred=xhat^2/20;
    v = y(k)-ypred;
    K= Pest*h'*(h*Pest*h'+R)^(-1);
    xhat=xhat+K*v;
    Pest=(1-K*h)*Pest;
    xekf(k)=xhat;
  
end
MSE1=(1/T*(sum(xpf-xTrue).^2))^(1/2);
MSE2=(1/T*(sum(xekf-xTrue).^2))^(1/2);
MSEpf(m)=MSE1;                                %第m次PF濾波的均方誤差
MSEekf(m)=MSE2;                               %第m次EKF濾波的均方誤差
m=m+1;
end
time = toc

figure(1)
clf;
 %plot(1:T,xTrue,'b*-',1:T,xpf,'r^-'); % 真實(shí)值藍(lán)色*表示，濾波值紅色三角表示
 plot(1:T,xTrue,'b*-');
% hold on
% plot(1:T,xpf,'r^-');
legend('原始值');
xlabel('Time');
axis([0 60 -40 40]);

figure(2)
clf;
plot(1:T,xTrue,'b*-',1:T,xpf,'r^-',1:T,xekf,'ko-'); % 真實(shí)值藍(lán)色*表示，粒子濾波值紅色三角表示 ，EKF濾波用綠色加號(hào)表示
% plot(1:T,xTrue,'b*-');
% hold on
% plot(1:T,xpf,'r^-');
legend('原始值','particle濾波值','EKF 濾波值');
xlabel('k');
axis([0 60 -40 40]);
figure(3)
clf;
plot(1:M,MSEpf,'r^-',1:M,MSEekf,'ko-');
xlabel('the count');
ylabel('MSE');
legend('PF MSE','EKF MSE');
figure(4)
clf;
plot(xpf,xTrue,'+');
hold on;
c=-25:1:25;
plot(c,c,'r');
axis([-25 25 -25 25]);
% op=std(xpf-xTrue)