rwm.m

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

function [xMH,pPro]=rwm(xSamples,S,xPre,q,t,y);
%功能：用MH法Random-Walk Metropolis对大误差估计点进行校正 070528
%输入：-xSamples=原粒子群
%      -numSamples=粒子数
%输出：-xMH=经MH算法得到的新的粒子群；
%      -pPro=对应权值；

if nargin < 6, error('Not enough input arguments.'); end

% sMH = zeros(S,1);    %MH法候选值步长          

pPro= zeros(S,1);

R=1;
% xMH(:,1)=xSamples(:,t);
N=2;
samplesMH=zeros(S,N);
samplesMH(:,1)=xSamples(:,t);
% % u=rand(S,1); 
% % sMH(:,1) = normrnd(S);
% % for i=1:S,
% % samplesMH(i,1) = xMH(i,1)+sMH(i,1);
% % xm=(samplesMH(i,1).^(2))./20;
% % xm= exp(-.5*R^(-1)*(y(t,1,1)-xm).^(2));
% % m = (xPre(i,t).^(2))./20;
% % m = exp(-.5*R^(-1)*(y(t,1,1)-m).^(2));
% % rate=xm/m;
% %   if u(i,1)<=rate
% %   samplesMH(i,1)=xSamples(i,t);
% %   else
% %   samplesMH(i,1)= samplesMH(i,1);   
% %   end;
% % end;

for I=2:N,
u=rand(S,1); 
sMH = normrnd(0,0.2,S);
for i=1:S,
xm=samplesMH(i,I-1)+sMH(i,1);
xProp= exp(-.5*R^(-1)*(y(t,1,1)-xm).^(2));
m = samplesMH(i,I);
mProp = exp(-.5*R^(-1)*(y(t,1,1)-m).^(2));
rate=xProp/mProp;
  if u(i,1)<=rate
  samplesMH(i,I)=xm;
  else
  samplesMH(i,I)= samplesMH(i,I-1);   
  end;
end;
end;

% M=10;
% for i=M:N,
%     sMH(i)
    
for i=1:S,
%     xMH(i,1)=sum(samplesMH(i,M:N))/(N-M);
    xMH(i,1)=sum(samplesMH(i,:))/N;
    xm=(xMH(i,1).^(2))./20;
    pPro(i,1)= exp(-.5*R^(-1)*(y(t,1,1)-xm).^(2));
    pPro(i,1) = q(i,t-1).*(pPro(i,1));
end;



% 权值归一化
% ==========
pSum = sum(pPro(:,1));
pPro(:,1) = pPro(:,1) / pSum;