clear;
echo off;

 
%-----------------Parameter--------------------------
xMEAN=4.5;xSIGMA=0.2;              %初始化采樣粒子的位置和速度                
yMEAN=4;ySIGMA=0.3;
VxMEAN=0.2;VxSIGMA=0.01;             
VyMEAN=0.15;VySIGMA=0.02;

UxMEAN=0;UxSIGMA=0.015625;          % 初始化x方向和y方向噪聲，均為零均值高斯白噪聲
UyMEAN=0;UySIGMA=0.015625;          % 標(biāo)準(zhǔn)差 為 0.015625 
UzMEAN=0;UzSIGMA=0.001;             

rSTEP=30;                         % 采樣30個(gè)時(shí)刻
wk=-1/(2*UzSIGMA);                % 粒子權(quán)重常數(shù)
wj=1/sqrt(2*pi*UzSIGMA);          % 粒子權(quán)重常數(shù)1/sqr(2*pi)
rN=1024;                          % 粒子數(shù)為1024

w_buffer=zeros(rN,1);             % 存儲(chǔ)權(quán)值的空間
r_buffer=zeros(rN,1);             % 存儲(chǔ)復(fù)制次數(shù)的空間
i_buffer=zeros(rN,1);             % 存儲(chǔ)粒子指針的空間

tX=zeros(rSTEP,1);tY=zeros(rSTEP,1);tVx=zeros(rSTEP,1);tVy=zeros(rSTEP,1);tZ=zeros(rSTEP,1);  %每一時(shí)刻狀態(tài)的估計(jì)值
rX=zeros(rSTEP,1);rY=zeros(rSTEP,1);rVx=zeros(rSTEP,1);rVy=zeros(rSTEP,1);                    %每一時(shí)刻狀態(tài)的真實(shí)值
x0=4.5;y0=4.5;Vx0=0.2;Vy0=0.2;           % 初始化真實(shí)狀態(tài)的初值


%-----------------真實(shí)狀態(tài)值和觀測(cè)角度值-----------------------------
tX(1,1)=x0;tY(1,1)=y0;tVx(1,1)=Vx0;tVy(1,1)=Vy0;
w0=gauss(UxMEAN,UxSIGMA,rSTEP);                      %噪聲
w1=gauss(UyMEAN,UySIGMA,rSTEP);
w2=gauss(UzMEAN,UzSIGMA,rSTEP);
tZ(1,1)=atan(tY(1,1)./tX(1,1))+w2(1,1);            %初始測(cè)量角          
for t=2:rSTEP
    tX(t,1)=tX(t-1,1)+tVx(t-1,1)+w0(t,1);          % tX(t,1)用來(lái)存儲(chǔ)目標(biāo)在x方向上的真實(shí)坐標(biāo)值
    tY(t,1)=tY(t-1,1)+tVy(t-1,1)+w1(t,1);          % tY(t,1)用來(lái)存儲(chǔ)目標(biāo)在y方向上的真實(shí)坐標(biāo)值
    tVx(t,1)=tVx(t-1,1)+0.5.*w0(t,1);              % tVx(t,1)用來(lái)存儲(chǔ)目標(biāo)在x方向上的真實(shí)速度值
    tVy(t,1)=tVy(t-1,1)+0.5.*w1(t,1);              % tVy(t,1)用來(lái)存儲(chǔ)目標(biāo)在y方向上的真實(shí)速度值
    tZ(t,1)=atan(tY(t,1)./tX(t,1))+w2(t,1);        % tZ(t,1)用來(lái)存儲(chǔ)每一時(shí)刻的測(cè)量角
end;

%---------------初始化----------------------------
x_buffer=gauss(xMEAN,xSIGMA,rN);       
Vx_buffer=gauss(VxMEAN,VxSIGMA,rN);    
y_buffer=gauss(yMEAN,ySIGMA,rN);     
Vy_buffer=gauss(VyMEAN,VySIGMA,rN);
for i=1:rN
    w_buffer(i,1)=1/rN;                % 初始權(quán)重值
    r_buffer(i,1)=1;                   % 初始復(fù)制次數(shù)
    i_buffer(i,1)=i;                   % 初始粒子指針
end;
iR=rN;                                 % 需重采樣粒子數(shù)初始為rN
    
%------------------粒子濾波-------------------------------
for t=1:rSTEP  
   %----------------采樣----------------------------
    indr=1;indd=rN;
    w0=gauss(UxMEAN,UxSIGMA,rN);
    w1=gauss(UyMEAN,UySIGMA,rN);
    reg=zeros(1,4);
    for indr=1:iR
       x=i_buffer(indr,1);           % 粒子指針
       reg(1,1)=x_buffer(x,1);       % reg用來(lái)存儲(chǔ)每一時(shí)刻目標(biāo)x,y方向上的估計(jì)坐標(biāo)值和x,y方向上的的估計(jì)速度值
       reg(1,2)=Vx_buffer(x,1);      % 
       reg(1,3)=y_buffer(x,1);
       reg(1,4)=Vy_buffer(x,1);
     
       x_buffer(x,1)=reg(1,1)+reg(1,2)+w0(indr,1); %
       Vx_buffer(x,1)=reg(1,2)+0.5.*w0(indr,1); 
       y_buffer(x,1)=reg(1,3)+reg(1,4)+w1(indr,1); 
       Vy_buffer(x,1)=reg(1,4)+0.5.*w1(indr,1); 
     
       for k=r_buffer(indr,1)-1:-1:1
           x_buffer(i_buffer(indd,1),1)=reg(1,1)+reg(1,2)+w0(indd,1);   %復(fù)制粒子
           Vx_buffer(i_buffer(indd,1),1)=reg(1,2)+0.5.*w0(indd,1);  
           y_buffer(i_buffer(indd,1),1)=reg(1,3)+reg(1,4)+w1(indd,1); 
           Vy_buffer(i_buffer(indd,1),1)=reg(1,4)+0.5.*w1(indd,1); 
           indd=indd-1;
      end;
   end;    

  %----------------權(quán)值計(jì)算---------------------

   W=0;
   for i=1:rN;
      x=tZ(t,1)-atan(y_buffer(i,1)./x_buffer(i,1));     %計(jì)算公式見(jiàn)報(bào)告中式1.5
      w_buffer(i,1)=exp(wk*x*x);                       %第i個(gè)粒子的權(quán)值
      W=W+w_buffer(i,1);                                %權(quán)值的累加和
  end;
%----------------狀態(tài)輸出---------------------------
   X=0;Vx=0;Y=0;Vy=0;
   for i=1:rN;
      X=X+w_buffer(i,1).*x_buffer(i,1);            % 計(jì)算x方位值
      Vx=Vx+w_buffer(i,1).*Vx_buffer(i,1);         % 計(jì)算x方向速度值
      Y=Y+w_buffer(i,1).*y_buffer(i,1);            % 計(jì)算y方位值
      Vy=Vy+w_buffer(i,1).*Vy_buffer(i,1);         % 計(jì)算y方向速度值
  end; 
   rX(t,1)=X/W;                                    % 輸出值歸一化
   rVx(t,1)=Vx/W;
   rY(t,1)=Y/W;
   rVy(t,1)=Vy/W;
 
  %----------------重采樣-------------------------
  indr=1;indd=rN;                       %設(shè)置指針的初始值
  K=rN/W;                               %計(jì)算中間變量K
  U=rand(1,1);                          %生成一個(gè)隨機(jī)閾值
  for i=1:rN;                           %主循環(huán)
      temp=K.*w_buffer(i,1)-U;          %添加一個(gè)中間變量temp
      r_buffer(indr,1)=quzheng(temp);   %
      U=r_buffer(indr,1)-temp;          %更新閾值
      if r_buffer(indr,1)>0             %
         i_buffer(indr,1)=i;            %存儲(chǔ)被復(fù)制粒子的地址
         indr=indr+1;
      else
         i_buffer(indd,1)=i;            % 存儲(chǔ)被拋棄粒子的地址
         indd=indd-1;                   
     end;
  end;
  iR=indr-1;
 %--------------------------------------------- 
end;

%------------------作圖--------------------------
figure(1);
plot(tX,tY,'-.g*',rX,rY,'-ro');
legend('true','estimate');