rsrandsir.m

自举粒子滤波器和改进后的自举粒子滤波器的比较应用

clear all;
close all;
UzSIGMA=0.001; 
rSTEP=50;                         % 采样30个时刻
wk=-1/(2*UzSIGMA);                % 粒子权重常数
wj=1/sqrt(2*pi*UzSIGMA); % 粒子权重常数1/sqr(2*pi)
n=1:500;
rN=length(n); % 粒子数为1024
stdw=sqrt(10);

%x0 = randn(1,1);
%c0 = 1;
%xpath = zeros(1,rN);
%xpath(1) = x0/2 + 25*x0/(1+x0^2) +8*cos(1.2) + stdw*randn(1,1);
%for i=2:rN
%xpath(i) = xpath(i-1)/2 + 25*xpath(i-1)/(1+xpath(i-1)^2) +8*cos(1.2*i) + stdw*randn(1,1);
%end
%zpath = 1/20*(xpath.^2) + randn(1,rN);
tX=zeros(rSTEP,1);tZ=zeros(rSTEP,1);
rX=zeros(rSTEP,1);
x0=rand(1,1);
tX(1)=x0;
tZ(1)=1/20*((tX(1))^2)+randn(1,1);  
for t=2:rSTEP
    tX(t)=tX(t-1)/2 + 25*tX(t-1)/(1+tX(t-1)^2)+8*cos(1.2)+ stdw*randn(1,1);%end
     tZ(t)=1/20*((tX(t))^2)+randn(1,1);
    end;
    
w_buffer=zeros(rN,1);             % 存储权值的空间
r_buffer=zeros(rN,1);             % 存储复制次数的空间
i_buffer=zeros(rN,1);             % 存储粒子指针的空间





x_buffer=randn(rN,1);
x_buffer(1)=randn(1,1);

for i=1:rN
    w_buffer(i,1)=1/rN;                % 初始权重值
    r_buffer(i,1)=1;                   % 初始复制次数
    i_buffer(i,1)=i;                   % 初始粒子指针
end;
iR=rN; 

%------------------粒子滤波-------------------------------
for t=1:rSTEP  
 %----------------采样----------------------------
    indr=1;indd=rN;
     reg=zeros(1,1);
     for indr=1:iR
          x=i_buffer(indr,1);           % 粒子指针 第几个粒子
       reg=x_buffer(x,1); 
       x_buffer(x,1)=reg/2+25*reg/(1+reg^2)+8*cos(1.2)+stdw*randn(1,1);
       
        for k=r_buffer(indr,1)-1:-1:1
             x_buffer(i_buffer(indd,1),1)=reg/2+25*reg/(1+reg^2)+8*cos(1.2)+stdw*randn(1,1);   %复制粒子
             indd=indd-1;
              end;
   end;    
   
    %----------------权值计算---------------------
  
   W=0;
   for i=1:rN;
        x=tZ(t,1)-1/20*(x_buffer(i))^2;     %计算公式见报告中式1.5
      w_buffer(i,1)=exp(wk*x*x);    
      W=W+w_buffer(i,1);                                %权值的累加和
  end;
%----------------状态输出---------------------------
X=0;
 for i=1:rN;
      X=X+w_buffer(i,1).*x_buffer(i,1); % 计算x方位值
      end; 
   rX(t)=X/W;
    %----------------重采样-------------------------
  indr=1;indd=rN;                       %设置指针的初始值
  K=rN/W;                               %计算中间变量K
  U=rand(1,1);                          %生成一个随机阈值
  for i=1:rN;                           %主循环
      temp=K.*w_buffer(i,1)-U;          %添加一个中间变量temp
      r_buffer(indr,1)=quzheng(temp);   %
      U=r_buffer(indr,1)-temp;          %更新阈值
      if r_buffer(indr,1)>0             %
         i_buffer(indr,1)=i;            %存储被复制粒子的地址
         indr=indr+1;
      else
         i_buffer(indd,1)=i;            % 存储被抛弃粒子的地址
         indd=indd-1;                   
     end;
  end;
  iR=indr-1;
 %--------------------------------------------- 
end;
ErrorRSR=sqrt(1/500*(rX-tX)'*(rX-tX));
%------------------作图--------------------------
figure(1);
hold on;
plot(tX,'g*');
plot(rX,'-r');
xlabel('time');
ylabel('x value');