%Particle Filter simulation 2008 Author:yx
%% Compare EKF and PF(bootstrap filter)
%Reference article Gordonnovelapproach.pdf Adapted from Gordon, Salmond, and Smith paper
% Ref: 1993 Novel approach to nonlinear/non-Gaussian Bayesian estimation
% example 2 PF_UKF_EKF_1_2a01.m
% Bearing-only tracking example
% 20081110 UKF
%本例對基于量測非線性模型（正切），進行了仿真；通過對比分析EKF，UKF和PF粒子濾%波的性能。仿真結果可以看出粒子濾波器比UKF優越，UKF比EKF性能優越。可作為學習%濾波器的參考資料。

clear all;
close all;
clc;%Clear command window.

Q = 0.001^2; % process noise covariance
R = 0.005^2; % measurement noise covariance
N = 100;% The number of particles in the particle filter
st = 30; % simulation length(time)

eps = 1.0e-017;
x0 = [-0.05000,0.00100,0.70000,-0.055000]'; % initial state
%x = [0.0,0.0,0.40000,-0.05]'; 
x  = x0;
xA = [x(1)];%Array:Save the true X -position
yA = [x(3)];%Array:Save Y-Position 
ZA = [];
%初始化系統方程系數
    F=[ 1.0 1.0 0.0 0.0;
        0.0 1.0 0.0 0.0;
        0.0 0.0 1.0 1.0;
        0.0 0.0 0.0 1.0];
    G=[0.5 0.0; 
       1.0 0.0;
       0.0 0.5;
       0.0 1.0];
%%%%%%%%%%%%
for k = 1 : st
    %two equation
    x = F * x + G * sqrt(Q)*[randn,randn]';%狀態方程
    z = atan(x(3)/x(1)) + sqrt(R) * randn; %觀測方程    
    xA = [xA x(1)];
    yA = [yA x(3)];
    ZA = [ZA z];
end
k = 0:st;
figure(1);
plot(xA,yA,'b*',0,0,'ro');
xlabel('x'); ylabel('y');
legend('Target Position','Observation Position'); 
%%%%%%%%%%%%%%%%%%%%%%%%
%pause
%for jj = 1:1%%%運動次數    
P  = [0.5,0,0,0;
    0,0.005,0,0;
    0,0,0.3,0;
    0,0,0,0.01].^2;   %協方差矩陣初始化
PA = [P(1)];%協方差矩陣
PAy = [P(11)];%協方差矩陣
KA = [[0,0,0,0]'];%增益

xge = x0;%EKF(擴展卡爾曼濾波器): the estimation of x--state data
xgeA = [xge(1)];%EKF(擴展卡爾曼濾波器): X -position
ygeA = [xge(3)];% Array:Save           Y-Position
Z1a = [0];
Z2a = [0];
%%%%%%%%%%%%%%%%%%%%二階
P2  = [0.5,0,0,0;
    0,0.005,0,0;
    0,0,0.3,0;
    0,0,0,0.01].^2;   %協方差矩陣初始化
xge2 = x0;%EKF(擴展卡爾曼濾波器): the estimation of x--state data
xgeA2 = [xge2(1)];%EKF(擴展卡爾曼濾波器): X -position
ygeA2 = [xge2(3)];% Array:Save           Y-Position
%%%%%%%%%%%%%%%%UKF%%%%%%%%%%%%%%
Pu  = [0.5,0,0,0;
    0,0.005,0,0;
    0,0,0.3,0;
    0,0,0,0.01].^2;   %協方差矩陣初始化
xgeu = x0;%
xgeAu = [xgeu(1)];%
ygeAu = [xgeu(3)];% 
alpha = 0.01;%0.5;%
beta  = 2;
nnn   = 4;
rmda  = alpha^2*nnn - nnn;%alpha=0.5,beta  = 2;kappa =3 -n %%3-nnn;%

%%%一般的方法：W0=v/(v+n),Wi=0.5/(v+n),i=1,...,2n;一般(v+n)==3
%%%這兒n=4,v=-1;W0=-1/3,Wi=1/6
wm    = -1/3;
wc    = -1/3;
for i = 1:2*nnn
    t = 1/6;
    wm =[ wm t];
    wc =[ wc t];
end 
% %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize the particle filter.
Pp = mean([[-0.05000,0.00100,0.70000,-0.055000]'-[0.0,0.0,0.40000,-0.05]']*[[-0.05000,0.00100,0.70000,-0.055000]-[0.0,0.0,0.40000,-0.05]]);
xgp = x0;

for i = 1 : N
    l=[sqrt(Q)*randn,sqrt(Q)*randn,sqrt(Q)*randn,sqrt(Q)*randn]';
    %l=[sqrt(Pp(1))*randn,sqrt(Pp(2))*randn,sqrt(Pp(3))*randn,sqrt(Pp(4))*randn]';
    xp(:,i) = xgp;%每步仿真有N個粒子+ l
    %xp(:,i) = xgp;
end   %如果初始化粒子加上上面的l，

xgpXA =[xgp(1)];
xgpYA =[xgp(3)];
   
for k = 1 : st
    %%%%%%%%%%%%%一階 Extended Kalman filter:注意方程的順序很重要
    %預測
     %1、求預測xge(k+1|k)，其它都是在預測的基礎上進行的，如果F可變則先求F(k|xge(k|k))
     %xtemp  = xge;
     xge = F * xge; 
     %2、求預測的觀測量z(k+1|k)=H(k+1)*xge(k+1|k)，
     H =[-xge(3)/(xge(3)^2+xge(1)^2),0,xge(1)/(xge(3)^2+xge(1)^2),0];%對非線性化理解不深，導數法，差分法，最優線性化等
     P = F * P * F' + G*Q*G';%P(k/k-1)=FP(k-1|k-1)F'+Q  % 狀態預測協方差矩陣
     K = P * H' * (H * P * H' + R)^(-1);   
   % H * P * H.'
    
    %更新xg(k) = xg(k/k-1)+K(y(k)-yg(k));
    z1 = ZA(k) - atan(xge(3)/xge(1));%殘差信息---新息
    
    Z1a = [Z1a z1];
    %z1 = z - (atan(xge(3)/xge(1))-H*(xge-xtemp));%殘差信息---新息
    xge = xge + K * z1;% xge(k+1|k+1)
    %P = (1 - K * H) * P;%P(k|k)
   % I4=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
    P = (eye(4) - K * H) * P;%*(I4 + K * H)'-K*R*K';%P(k|k)    
    % Save data in arrays for later plotting
    xgeA = [xgeA xge(1)];
    ygeA = [ygeA xge(3)];
    PA =[PA P(1)]; 
    PAy = [PAy P(11)];
    
    KA = [KA K];
    %%%%%%%%%%%%%二階 Extended Kalman filter:注意方程的順序很重要
    %預測
%     xge2 = F * xge2; 
%     H2 =[-xge2(3)/(xge2(3)^2+xge2(1)^2),0,xge2(1)/(xge2(3)^2+xge2(1)^2),0];%對非線性化理解不深，導數法，差分法，最優線性化等
%     P2 = F * P2 * F' + G*Q*G';%P(k/k-1)=FP(k-1|k-1)F'+Q  % 狀態預測協方差矩陣
%     K2 = P2 * H2' * (H2 * P2 * H2' + R)^(-1);   
% 
%     H221 = [2*xge2(3)*xge2(1)/(xge2(3)^2+xge2(1)^2)^2,0,-1/(xge2(3)^2+xge2(1)^2)+2*xge2(3)^2/(xge2(3)^2+xge2(1)^2)^2,0];
%     H222 = [0,0,0,0];
%     H223 = [1/(xge2(3)^2+xge2(1)^2) - 2*xge2(1)^2/(xge2(3)^2+xge2(1)^2)^2,0,-2*xge2(3)*xge2(1)/(xge2(3)^2+xge2(1)^2)^2,0];
%     H224 = [0,0,0,0];
%     e1 =[1,0,0,0];e2 =[0,1,0,0];e3 =[0,0,1,0];e4 =[0,0,0,1];    
%     temp2 = 0.5*(e1*trace(H221*P2)'+e2*trace(H222*P2)'+e3*trace(H223*P2)'+e4*trace(H224*P2)')
%     %更新xg(k) = xg(k/k-1)+K(y(k)-yg(k));
%     z12 = ZA(k) - atan(xge2(3)/xge2(1))-temp2;%殘差信息---新息
% 
%     xge2 = xge2 + K2 * z12;% xge(k+1|k+1)       
%     % Save data in arrays for later plotting
%     xgeA2 = [xgeA2 xge2(1)];
%     ygeA2 = [ygeA2 xge2(3)];
    %%%%%%%%%%%%%%UKF濾波器%%%%%%%%%%%%%%%%%%%%%%n=4,a=0.01,b=2,r=4*0.01^2-4 ,w0m=
    %%%wxx(k-1)
   wxx = [xgeu];
    for j = 1:nnn
        tt = (nnn + rmda)*Pu(:,j);
        t = xgeu + sqrt(tt);
        wxx = [wxx t];
    end    
    for j =1:nnn
        tt = (nnn + rmda)*Pu(:,j);
        t = xgeu - sqrt(tt);
        wxx = [wxx t];
    end
        
    %%%wxx(k),xgeu(k|k-1),Pu(k|k-1)
    for j = 1:2*nnn+1
        wxx(:,j) = F * wxx(:,j);
    end
      
%     xgeu = wm(1) * wxx(:,1);
%     for j = 2:2*nnn+1
%         xgeu = xgeu + wm(j) * wxx(:,j);
%     end
%     Pu = Q*eye(4);
%     for j = 1:2*nnn+1
%         Pu = Pu + wc(j)*(wxx(:,j) - xgeu)*(wxx(:,j) - xgeu)';
%     end
    %%%%%%%%%%%%%%%%
    xgeu = F * xgeu;     
    Pu = F * Pu * F' + G*Q*G';
    %%%%傳播
    wxx = [xgeu];
    for j = 1:nnn
        t = xgeu + sqrt(((nnn + rmda)*Pu(:,j)));
        wxx = [wxx t];
    end    
    for j =1:nnn
        t = xgeu - sqrt(((nnn + rmda)*Pu(:,j)));
        wxx = [wxx t];
    end
    %%%%%Zkk    
    for j = 1:2*nnn+1
        wzz(j) = atan(wxx(3,j)/wxx(1,j));
    end
    
    zku = wm(1) * wzz(1);
    for j = 2:2*nnn+1
        zku = zku + wm(j) * wzz(j);
    end
    
    Pzu = R;
    for j = 1:2*nnn+1
        Pzu = Pzu + wc(j)*(wzz(j) - zku)*(wzz(j) - zku)';
    end
    
    Pxzu = wc(1)*(wxx(:,1) - xgeu)*(wzz(1) - zku)';
    for j = 2:2*nnn+1
        Pxzu = Pxzu + wc(j)*(wxx(:,j) - xgeu)*(wzz(j) - zku)';
    end
    %%%%%更新
    Ku = Pxzu*inv(Pzu);
    xgeu = xgeu + Ku*( ZA(k) - zku);
    Pu = Pu - Ku * (Pzu) * Ku';    %(Pzu)^(-1) Error
    
    xgeAu = [xgeAu xgeu(1)];%
    ygeAu = [ygeAu xgeu(3)];% 
    %%%%%%%%%%%%%%bootstrap粒子濾波器%%%%%%%%%%%%%
   %產生新的粒子
    for i = 1 : N
        %l = F*xp(:,i);
        xpminus(:,i) = F*xp(:,i) + G*sqrt(Q)*[randn,randn]';
        %0.5 * xp(i) + 25 * xp(i) / (1 + xp(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%xp(k-1)(i)
        l = xpminus(3,i)/(xpminus(1,i)+eps);
        yp = atan(xpminus(3,i)/(xpminus(1,i)));
        %xpminus(i)^2 / 20;
        vg = ZA(k) - yp;%觀測和預測的差
        q(i) = (1.0 / sqrt(2.0*pi*R)) * exp(-0.5*vg^2 * inv(R)) + eps;%概率 v(k)--vg
    end 
  % 歸一化每個先驗估計的概率(權重) Normalize the likelihood of each a priori estimate.
    qsum = sum(q);
    q = q./qsum;
%     for i = 1 : N
%         if qsum ~= 0
%             q(i) = q(i) / qsum;%歸一化權重
%         else
%             q(i) = 1/N;
%         end
%     end
    % 重采樣 Resample.
    for i = 1 : N
        u = rand; %均勻隨機數 uniform random number between 0 and 1
        qsum = 0;
        for j = 1 : N
            qsum = qsum + q(j);
            if qsum >= u
                xp(:,i) = xpminus(:,j);%xp(k)(i)
                break;
            end
        end
    end    
    % 獲得粒子濾波器估計，均值The particle filter estimate is the mean of the particles.
    xgp = mean(xp,2);
    Pp  = var(xp,0,2);
    % Save data in arrays for later plotting
    xgpXA = [xgpXA xgp(1)];
    xgpYA = [xgpYA xgp(3)];
%    PpA = [PpA Pp];
   %%%%%%%%%%%%%   
end

k = 0:st;
figure;
plot(xA,yA,'b*-',xgeA,ygeA,'ro:',xgeAu,ygeAu,'g+:',xgpXA,xgpYA,'kx:');%xA,yA,'b*',
xlabel('x'); ylabel('y');
legend('True','EKF','UKF','PF'); 
set(gcf,'Color','White'); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure;
% plot(xA,yA,'b*-',xgeA,ygeA,'ro:');%xA,yA,'b*',
% xlabel('x'); ylabel('y');
% legend('True','EKF'); 
% set(gcf,'Color','White'); 
% 
% figure;
% plot(xA,yA,'b*-',xgeAu,ygeAu,'ro:');%xA,yA,'b*',
% xlabel('x'); ylabel('y');
% legend('True','UKF'); 
% set(gcf,'Color','White'); 
% 
% figure;
% plot(xA,yA,'b*-',xgpXA,xgpYA,'ro:');
% xlabel('x'); ylabel('y');
% legend('True','PF-1'); 
% set(gcf,'Color','White'); 
% %k = 1:st;
% figure;
% plot(k,Z1a,'go',k,PA,'b*-',k,PAy,'r.-');
% xlabel('k');% ylabel('y');
% legend('殘差','x誤差','y誤差');
% set(gcf,'Color','White'); 
% 
% k = 1:st;
% figure;
% plot(k,(180/pi)*ZA(k),'r.-');
% xlabel('k'); %ylabel('y');
% legend('測量實際值(單位度)');
% set(gcf,'Color','White'); 
%legend('True','PF-1'); (5.729577951308232e+001)*

%pause
%end