errorcorrect.m

采用误差校正算法改进的粒子滤波器

function []=errorCorrect();
% 功能：对大误差点进行比例调节校正
% 输入：
% 输出：

%调节方法9 比例法 070210
%==========================================================================

%对估计值和预测值进行正负校对

if tCr1(t+2,1)>3&&tCr2(t+3,1)>2 || tCr1(t+2,1)>2&&tCr2(t+3,1)>5
    wPre(t+1)=-wPre(t+1);
end;
if tCr1(t+2,2)>3&&tCr2(t+3,2)>2 || tCr1(t+2,2)>2&&tCr2(t+3,2)>5
    wPost(t+1)=-wPost(t+1);
end;

%根据预测测量误差和估计测量误差按比例调节估计值

tempCr1(t+1,1)=sqrt(((wPre(t+1).^(2))./20-y(t+1)).^2);
 if Cr(t+1,1)>2  && abs(Cr(t,1)-Cr(t+1,1))>0.5 
    
      
      if Cr(t+1,1)>tempCr1(t+1,1)
          tempCr1(t+1,2)=1;
      else
          Cr(t+1,2)=1;
      end;
          xCr(t+1,1)=(wPost(t+1)*tempCr1(t+1,1)+wPre(t+1)*Cr(t+1,1))/(Cr(t+1,1)+tempCr1(t+1,1)); 
          tempCr1(t+1,2)=1;
          tempCr2(t+1,1)=sqrt(((xCr(t+1,1).^2)./20-y(t+1)).^2);
          xCr1(t+1,1)=(wPost(t+1)*tempCr1(t+1,1)-wPre(t+1)*Cr(t+1,1))/(tempCr1(t+1,1)-Cr(t+1,1)); 
          tempCr3(t+1,1)=sqrt(((xCr1(t+1,1).^(2))./20-y(t+1)).^2);
          if tempCr2(t+1,1)>10  ||  tempCr3(t+1,1)<tempCr2(t+1,1)
             xCr(t+1,1)=xCr1(t+1,1);
             tempCr2(t+1,1)=tempCr3(t+1,1);
          end;
                      
%           if tempCr3(t+1,1)>tempCr1(t+1,1)
%               xCr(t+1,1)=wPost(t+1)-(wPre(t+1)-wPost(t+1))*(1-tempCr1(t+1
%               ,1)/Cr(t+1,1))*0.8;
%           end; 
        
  else
      if Cr(t+1,1)>1 &&tempCr1(t+1,1)>2
          xCr(t+1,1)=wPost(t+1);
      else
          xCr(t+1,1)=wPost(t+1);  
      end;
end;
end;

xCr(1,1)=wPost(1);
xCr(100,1)=wPost(100);