gpf.asv

基于粒子滤波实现的纯方位目标跟踪

clear;
echo off;

 
%-----------------Parameter--------------------------
xMEAN=4.5;xSIGMA=0.1;                % 位置初始分布p(x)=p(y)=(35,25)              
yMEAN=4;ySIGMA=0.2;
VxMEAN=0.2;VxSIGMA=0.01;             % 速度初始分布p(Vx)=p(Vy)=(2.5,1)
VyMEAN=0.15;VySIGMA=0.02;

UxMEAN=0;UxSIGMA=0.015625;          % x方向和y方向噪声均为零均值白噪声p(Ux)=p(Uy)=p(0,1)
UyMEAN=0;UySIGMA=0.015625;          %          方差 为 0.015625 ？
UzMEAN=0;UzSIGMA=0.001;             % 观测噪声p(Uz)=p(0,0.05)

rSTEP=30;                         % Number of time steps
wk=-1/(2*UzSIGMA);                % 粒子权重常数-1/(2*1*1*pi)是不是少个pi
wj=1/sqrt(2*pi*UzSIGMA);          % 粒子权重常数1/sqr(2*pi)
rN=1024;                           % 粒子数

w_buffer=zeros(rN,1);             % 权重
r_buffer=zeros(rN,1);             % 复制次数
i_buffer=zeros(rN,1);             % 粒子指针

tX=zeros(rSTEP,1);tY=zeros(rSTEP,1);tVx=zeros(rSTEP,1);tVy=zeros(rSTEP,1);tZ=zeros(rSTEP,1);
rX=zeros(rSTEP,1);rY=zeros(rSTEP,1);rVx=zeros(rSTEP,1);rVy=zeros(rSTEP,1);
x0=4.5;y0=4.5;Vx0=0.2;Vy0=0.2;           % 初始状态值
%-----------------真实状态值和观测值-----------------------------
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);
for t=2:rSTEP
    tX(t,1)=tX(t-1,1)+tVx(t-1,1)+w0(t,1);
    tY(t,1)=tY(t-1,1)+tVy(t-1,1)+w1(t,1);
    tVx(t,1)=tVx(t-1,1)+0.5.*w0(t,1);
    tVy(t,1)=tVy(t-1,1)+0.5.*w1(t,1);
    tZ(t,1)=atan(tY(t,1)./tX(t,1))+w2(t,1);
end;
%---------------Initialization-------------------------------------
for i=1:rN
    w_buffer(i,1)=1/rN;                % 初始权重值
    r_buffer(i,1)=1;                   % 初始复制次数
    i_buffer(i,1)=i;                   % 初始粒子指针
end;
%------------------particle filter-------------------------------
for t=1:rSTEP  
   %----------------sample----------------------------
    indr=1;indd=rN;
    w0=gauss(UxMEAN,UxSIGMA,rN);
    w1=gauss(UyMEAN,UySIGMA,rN);
    reg=zeros(1,4);
     x_buffer=gauss(xMEAN,xSIGMA,rN);       % States
     Vx_buffer=gauss(VxMEAN,VxSIGMA,rN);    
     y_buffer=gauss(yMEAN,ySIGMA,rN);     
     Vy_buffer=gauss(VyMEAN,VySIGMA,rN);
    for indr=1:rN
       x=i_buffer(indr,1);
      
       reg(1,1)=x_buffer(x,1);       % The replicated particle is stored in variable reg
       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); 
       % end;
   end;    
   %end;
  %----------------importance---------------------
 % z_buffer=tZ(t,1);
   W=0;
   for i=1:rN;
      x=tZ(t,1)-atan(y_buffer(i,1)./x_buffer(i,1));
      w_buffer(i,1)=exp(wk*x*x)*w_buffer(i,1);%*w_buffer(i,1);
      W=W+w_buffer(i,1);
  end;
%----------------output---------------------------
   X=0;Vx=0;Y=0;Vy=0;
   for i=1:rN;
      X=X+w_buffer(i,1).*x_buffer(i,1);            % 计算x方位值
      Vx=Vx+w_buffer(i,1).*Vx_buffer(i,1);         % 计算x方向速度值
      Y=Y+w_buffer(i,1).*y_buffer(i,1);            % 计算y方位值
      Vy=Vy+w_buffer(i,1).*Vy_buffer(i,1);         % 计算y方向速度值
  end; 
   xMEAN=X/W;
   VxMEAN=Vx/W;
   yMEAN=Y/W;
   VyMEAN=Vy/W;
 %  xi_buffer=(xMEAN.*ones(1024,1)-x_buffer);
   %yi_buffer=yMEAN.*ones(1024,1)-y_buffer;
  % Vxi_buffer=VxMEAN.*ones(1024,1)-Vx_buffer;
   %Vyi_buffer=VyMEAN.*ones(1024,1)-Vy_buffer;
  % xSIGMA=(xi_buffer'*xi_buffer)/rN;
   xSIGMA=0;
   VxSIGMA=0;
   ySIGMA=0; 
   VySIGMA=0;
   for i=1:rN;
       u1=xMEAN-x_buffer(i,1);
       u2=VxMEAN-Vx_buffer(i,1);
       u3=yMEAN-y_buffer(i,1);
       u4=VyMEAN-Vy_buffer(i,1);
   xSIGMA=xSIGMA+w_buffer(i,1)*u1*u1;
   VxSIGMA=VxSIGMA+w_buffer(i,1)*u2*u2;
   ySIGMA=ySIGMA+w_buffer(i,1)*u3*u3;
   VySIGMA=VySIGMA+w_buffer(i,1)*u4*u4;
  % VxSIGMA=(yi_buffer'*yi_buffer)/W;
   %ySIGMA=(Vxi_buffer'*Vxi_buffer)/W;
   %VySIGMA=(Vyi_buffer'*Vyi_buffer)/W;
end;
    xSIGMA=xSIGMA/W;
    VxSIGMA=VxSIGMA/W;
    ySIGMA=ySIGMA/W;
    VySIGMA=VySIGMA/W;
%bi=(xMEAN.*ones(1024,1)-x_buffer);
%ai=bi'*bi;
%next=sqrt(1/(2*pi*xSIGMA))*exp(-ai/(2*xSIGMA))

   %xSIGMA= xSIGMA/rN;
   rX(t,1)=xMEAN;                               % 输出值归一化
   rVx(t,1)=VxMEAN;
   rY(t,1)=yMEAN;
   rVy(t,1)=VyMEAN;
   
   end;
   RMSE=0;
   for t=1:rN;
       t=rVx(t,1)-tX(t,1)
       RMSE=RMSE+
  %----------------resample-------------------------
 % indr=1;indd=rN;                     % Set the intial values for indxes
%  K=rN/W;                             % Calculating the constant
  %U=rand(1,1);
 % for i=1:rN;            
   %   temp=K.*w_buffer(i,1)-U;          % Temporary varible
  %    r_buffer(indr,1)=quzheng(temp);   % Storing the replication factor
 %     U=r_buffer(indr,1)-temp;          % Updating the uniform random number
  %    if r_buffer(indr,1)>0             % Particle allocation
 %        i_buffer(indr,1)=i;            % Storing the address of the replicated particle
  %       indr=indr+1;
 %     else
%         i_buffer(indd,1)=i;            % Storing the  address of the discarded particle
    %     indd=indd-1;
%     end;
 % end;
 % iR=indr-1;
 %--------------------------------------------- 
end;


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