pfdemo.m

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

% 版本    ：2.0
% 时间    ：2007-1-30
% 作者    ：zll
% 功能    ：对下面的状态空间模型进行粒子滤波，使用SIR算法
%         ： x(t+1) = 0.5x(t) + [25x(t)]/[(1+x(t))^(2)] 
%         ：           + 8cos(1.2t) + process noise
%         ： y(t) =  x(t)^(2) / 20  + measurement noise
%         ：包括预测过程，权值递推及权值归一化过程，重采样过程，加入了是否进行重采样的判断
%         ：增加了均方差，显示更直观，用matlab表达式替代for循环，加快运行速度
% 基本思想：通过给定的状态初始值以及状态方程，测量方程，递推得到每一时刻的状态值和测量值。
%         ：然后根据得到的测量值利用SIS算法求得每一时刻状态的估计值，将估计值与真值进行对比。
%         ：重要性函数选为高斯分布函数。
%         ：通过误差校正来提高估计精度
                          

clear;
echo off;


% 初始化参数
% ==========

N = 100;                % 算法迭代的时间步数
t = 1:1:N;             % 时间序列  
x0 = 0.1;                % 状态初始值
n = 1;                 % z维的大小，缺省为1，当状态变量为多维时，n>1，此时可对z维中的每一层进行同步滤波
x = zeros(N,1,n);      % 状态变量初始化
y = zeros(N,1,n);      % 测量值初始化
x(1,1,n) = x0;         % 迭代初值 
R = 1;                 % 测量噪声方差
Q = 1;                 % 过程噪声方差     
initVar = 5;           % 状态初始方差  
numSamples = 500;      % 采样粒子数目
Nstd = 2;              % 误差条设置

Cr = zeros(N,2,n);     %测量误差方差真实值       00/070129
xCr = zeros(N,1,n);    %误差校正估计值           00/070130
xFCr = zeros(N,1,n);   %MH估计值           00/070208
wPre = zeros(N,1);

no_of_runs=100;       %执行次数

PxTomeanPost=zeros(no_of_runs,1);
PxTomeanWeight=zeros(no_of_runs,1);
PxToFCr=zeros(no_of_runs,1);
PxToCr=zeros(no_of_runs,1);

for r=1:no_of_runs,

r
% 产生过程噪声和测量噪声
% ======================

v = randn(N,1);
w = sqrt(Q)*randn(N,1);   


% 递推得到状态真值以及测量值
% ==========================

y(1,1) = (x(1,1)^(2))./20 + v(1,1); 
for t = 2:N,
  x(t,1) = 0.5*x(t-1,1) + 25*x(t-1,1)/(1+x(t-1,1)^(2)) + 8*cos(1.2*(t-1)) + w(t,1);
  y(t,1) = (x(t,1).^(2))./20 + v(t,1); 
end;

% 进行粒子滤波：SIR算法
% =====================

[rows,cols] = size(y);       
S = numSamples;              
xSamples = zeros(S,rows);     % 后验状态估计值初始化
xPre = zeros(S,rows);         % 状态预测值初始化
xMH = zeros(numSamples,1);    % MH法候选值 
q = 1/S.*ones(S,rows);        % 重要性权值初始化
qF = 1/S.*ones(S,rows);        % 完全MH重要性权值初始化
% 初始采样
% ========

initSamples = sqrt(initVar)*randn(S,1);          
xPre(:,1) = predictstates(initSamples,1,Q);
q(:,1) = importanceweights(xPre(:,1),y(1,1),R,q(:,1));        
[xSamples(:,1),q(:,1)] = updatestates(xPre(:,1),q(:,1));             % 00/070129


% 预测与更新过程
% ==============

for t = 1:rows-1,  
  
  xPre(:,t+1) = predictstates(xSamples(:,t),t,Q);                            % 状态预测
  q(:,t+1) = importanceweights(xPre(:,t+1),y(t+1,1),R,q(:,t));               % 权值计算
  [xSamples(:,t+1), q(:,t+1)] = updatestates(xPre(:,t+1),q(:,t+1));          % 根据测量值进行状态更新和权值更新  %00/070129

end;

% 计算后验均值估计，极大后验估计，后验加权估计
% ============================================
samples=xSamples;
meanPost = mean(samples);               % 后验均值估计  
meanWeight = sum(samples .* q);         % 后验加权估计
PxTomeanPost(r,1) = sqrt(1/N * sum((x-meanPost').^2));     % 真实值与后验均值估计之间的均方差
PxTomeanWeight(r,1) = sqrt(1/N * sum((x-meanWeight').^2));  % 真实值与后验加权估计之间的均方差



% 获得误差真实值Cr(k)，利用测量误差与估计误差间的对应关系，调节估计值，提高估计精度
% ===================00/070129，30 ,070202

%计算估计值、预测值的一级预测测量误差和二级预测测量误差
%==========================================================================
tCr1=zeros(N+1,2);
yPre1=zeros(N+1,2);
yPre2=zeros(N+2,2);
tCr2=zeros(N+2,2);
tempCr1=zeros(N,2);
tempCr2=zeros(N,2);
tempCr3=zeros(N,2);
wPost = sum(xSamples.*q);

initSamples = sqrt(initVar)*randn(S,1);          
xPre(:,1) = predictstates(initSamples,1,Q);
q(:,1) = importanceweights(xPre(:,1),y(1,1),R,q(:,1));        
[xSamples(:,1),q(:,1)] = updatestates(xPre(:,1),q(:,1));             % 00/070531

% %=====================================================================
   % MH算法
for t = 2:rows,  
  [xFMH,qF(:,t)]=rwm(xSamples,S,xPre,q,t,y);
   xSamples(:,t)=xFMH ;
   xFCr(t,1)=sum(xSamples(:,t).*qF(:,t));
end;
xFCr(1,1)=sum(xSamples(:,1).*q(:,t));
xFCr(100,1)=sum(xSamples(:,100).*q(:,100));
PxToFCr(r,1)=sqrt(1/N * sum((x-xFCr).^2));     % 真实值与MH均值估计之间的均方差

% 预测与更新过程
% ==============
for t = 2:rows-2,  
  
  xPre(:,t+1) = predictstates(xSamples(:,t),t,Q);                            % 状态预测
  q(:,t+1) = importanceweights(xPre(:,t+1),y(t+1,1),R,q(:,t));               % 权值计算
  [xSamples(:,t+1), q(:,t+1)] = updatestates(xPre(:,t+1),q(:,t+1));          % 根据测量值进行状态更新和权值更新  %00/070129
  
   % %=====================================================================
   % 校正MH算法
  wPre(t) = 0.5.*meanWeight(t-1)+ 25.*meanWeight(t-1)/(1+meanWeight(t-1).^(2)) + 8*cos(1.2*(t-1)) ; % 计算一步和两步预测测量误差  00/070531
  wPre11 = 0.5.*wPre(t)+ 25.*wPre(t)/(1+wPre(t).^(2)) + 8*cos(1.2*(t)) ;
  wPre12 = 0.5.*wPost(t)+ 25.*wPost(t)/(1+wPost(t).^(2)) + 8*cos(1.2*(t)) ;
  yPre1(t+1,1) = (wPre11.^(2))./20;
  yPre1(t+1,2) = (wPre12.^(2))./20;
  tCr1(t+1,1)=sqrt((yPre1(t+1,1)-y(t+1)).^2); 
  tCr1(t+1,2)=sqrt((yPre1(t+1,2)-y(t+1)).^2); 
  wPre21 = 0.5.*wPre11+ 25.*wPre11/(1+wPre11.^(2)) + 8*cos(1.2*(t+1)) ;
  wPre22 = 0.5.*wPre12+ 25.*wPre12/(1+wPre12.^(2)) + 8*cos(1.2*(t+1)) ;
  yPre2(t+2,1) = (wPre21.^(2))./20;
  yPre2(t+2,2) = (wPre22.^(2))./20;
  tCr2(t+2,1)=sqrt((yPre2(t+2,1)-y(t+2)).^2);  
  tCr2(t+2,2)=sqrt((yPre2(t+2,2)-y(t+2)).^2);  
  
   if t>0
    M=1;
  else
    M=t;
  end;

  CrSum = 0;   
  for i=(t-M+1):t+1, 
  CrSum= CrSum+((wPost(i).^(2))./20-y(i)).^2;
  end;

  Cr(t+1,1)= sqrt(CrSum/M); 
  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;
     [xMH,q(:,t)]=rwm(xSamples,S,xPre,q,t,y);
     xSamples(:,t)=xMH ;
     xCr(t,1)=sum(xSamples(:,t).*q(:,t));

  else
       xCr(t,1)=sum(xSamples(:,t).*q(:,t));
end;
end;
xCr(1,1)=sum(xSamples(:,1).*q(:,t));
xCr(99,1)=sum(xSamples(:,99).*q(:,99));
xCr(100,1)=sum(xSamples(:,100).*q(:,100));
mPre=mean(xPre);
mPost=mean(xSamples);

% xCr(1,1)=wPost(1);
% xCr(N,1)=wPost(N);

 
PxToCr(r,1)=sqrt(1/N * sum((x-xCr).^2));     % 真实值与误差校正均值估计之间的均方差

% 极大后验估计
bins = 20;
xMap = zeros(N,1);
for t = 1:N,
  [p,pos] = hist(samples(:,t,1),bins);    
  map = find(p==max(p));
  xMap(t,1) = pos(map(1));                
end;
PxToxMap = sqrt(1/N * sum((x-xMap).^2));           % 真实值与极大后验估计之间的均方差
end;

ErrorMW=mean(PxTomeanWeight);
ErrorMH=mean(PxToFCr);
ErrorPMH=mean(PxToCr);
VarMW=var(PxTomeanWeight);
VarMH=var(PxToFCr);
VarPMH=var(PxToCr);
disp(' ');
disp('************* FINAL RESULTS *****************');
disp(' ');
disp('RWSE : mean and variance');
disp('---------');
disp(' ');
disp(['PF          = ' num2str(ErrorMW) ' (' num2str(VarMW) ')']);
disp(['PF-MH         = ' num2str(ErrorMH) ' (' num2str(VarMH) ')']);
disp(['PF-PMH           = ' num2str(ErrorPMH) ' (' num2str(VarPMH) ')']);


% 图形输出结果
% ============

% 绘制噪声分布图
% figure(1)
% clf;
% subplot(221)
% plot(v);
% ylabel('Measurement Noise','fontsize',15);
% xlabel('Time','fontsize',15);
% subplot(222)
% plot(w);
% ylabel('Process Noise','fontsize',15);
% xlabel('Time','fontsize',15);
% subplot(223)
% plot(x)
% ylabel('State x','fontsize',15);
% xlabel('Time','fontsize',15);
% subplot(224)
% plot(y)
% ylabel('Observation y','fontsize',15);
% xlabel('Time','fontsize',15);

% 绘制后验均值估计与真值对比图，采用误差条
% figure(2)
% clf;

% for t = 1:N,
% plot(t,meanPost(:,t),'ro',t,x(t,1),'go',t,Cr(t,1),'bo',t,eXtoPX(t),'y*');   
% plot(t,Cr(t,1),'b-o',t,eXtoPX(t),'r--o'); 
% hold on
% errorbar(t,meanPost(:,t),Nstd*std(samples(:,t)),Nstd*std(samples(:,t)),'k');
%   legend('Posterior mean estimate','True value');
%   ylabel('Sequential state estimate','fontsize',15);
%   xlabel('Time','fontsize',15);
% end;


%//0207

% //20070531 for t = 1:N,
%   eXtoPX(t)=(abs(x(t,1)-meanWeight(:,t)));
%   xY(t,1)=(x(t,1).^(2))./20 ;
%   mXtoPX(t)=(abs(xY(t,1)-y(t,1)));
% end;
%   plot(1:N,Cr(:,1),'b-*',1:length(eXtoPX),eXtoPX,'r--o',1:length(mXtoPX),mXtoPX,'k-'); 
%   hold on
% %   errorbar(t,meanPost(:,t),Nstd*std(samples(:,t)),Nstd*std(samples(:,t)),'k');
%   legend('measure-estimate error','estimate error','measure error');
%   ylabel('error','fontsize',15);
%  20070531 xlabel('Time','fontsize',15);
  
  
  
% % % figure(2)
% % % clf;

% for t = 1:N,
%     if tempCr1(t,2)==1
%     plot(t,meanWeight(:,t),'ko',t,x(t,1),'go',t,wPre(t),'bo',t,xCr(t,1),'b*',t,wPost(t),'yo'); 
%     hold on
%     errorbar(t,meanWeight(:,t),Nstd*std(samples(:,t)),Nstd*std(samples(:,t)),'k');
%   
%     else
%       if Cr(t,2)==1
%         plot(t,meanWeight(:,t),'ko',t,x(t,1),'go',t,wPre(t),'bo',t,xCr(t,1),'r*',t,wPost(t),'yo');   
%         hold on
%        errorbar(t,meanWeight(:,t),Nstd*std(samples(:,t)),Nstd*std(samples(:,t)),'k');
%       
%       end;
%     end;
%   legend('Posterior weigth estimate','True value','Pre value','estimate');
%   ylabel('Sequential state estimate','fontsize',15);
%   xlabel('Time','fontsize',15);
% end;
% 
% % for t=1:N,
% %   if Cr(t,1)>2 
% %   plot(t,meanWeight(:,t),'yo',t,x(t,1),'g*',t,wPost(t),'k*',t,xCr(t,1),'r*');
% % %   hold on
% % %   errorbar(t,meanWeight(:,t),Nstd*std(samples(:,t)),Nstd*std(samples(:,t)),'k');
% % %   legend('Posterior weigth estimate','True value','PF value','MHPF value');
% % %   ylabel('Sequential state estimate','fontsize',15);
% % %   xlabel('Time','fontsize',15);
% % end;
% % end;

% 绘制真实值与后验均值估计，极大后验估计，后验加权均值估计的对比图
% figure(3)
% clf;
% 
% plot(1:length(x),x,'g-*',1:length(x),meanPost,'r-+',1:length(x),xMap,'b-*',1:length(x),meanWeight,'c-s');
% legend('True value','Posterior mean estimate','MAP estimate','Mean estimate with weights');
% % plot(1:length(x),x,'g-*',1:length(x),meanPost,'r-+',1:length(y),y,'b-*');
% % legend('True value','Posterior mean estimate','Measu value');
% ylabel('State estimate','fontsize',15)
% xlabel('Time','fontsize',15)
% 
% % 绘制分别以后验均值估计，极大后验估计，后验加权均值估计为横坐标，真实值为纵坐标的点分布图
% figure(4)
% clf;
% 
% subplot(221)
% plot(meanPost,x,'+')
% ylabel('True state','fontsize',15)
% xlabel('Posterior mean estimate','fontsize',15)
% hold on
% c=-25:1:25;
% plot(c,c,'r');
% axis([-25 25 -25 25]);
% text(0,-15,'均方差=');
% text(10,-20,num2str(PxTomeanPost));
% hold off
% 
% subplot(222)
% plot(xMap,x,'+')
% ylabel('True state','fontsize',15)
% xlabel('MAP estimate','fontsize',15)
% hold on
% c=-25:1:25;
% plot(c,c,'r');
% axis([-25 25 -25 25]);
% text(0,-15,'均方差=');
% text(10,-20,num2str(PxToxMap));
% hold off
% 
% subplot(223)
% plot(meanWeight,x,'+')
% ylabel('True state','fontsize',15)
% xlabel('Mean estimate with weights','fontsize',15)
% hold on
% c=-25:1:25;
% plot(c,c,'r');
% axis([-25 25 -25 25]);
% text(0,-15,'均方差=');
% text(10,-20,num2str(PxTomeanWeight));
% hold off
% 
% % 绘制后验概率密度函数分布图
% [row,col]=size(q);
% domain = zeros(row,1);
% range = zeros(row,1);
% parameter = 1;
% bins = 10;
% support=[-20:1:20];
% 
% 
% figure(5)
% clf;
% 
% u=[0 1];
% caxis(u);
% for t=1:1:N,
%   figure(5)  
%   [range,domain]=hist(samples(:,t,parameter),support);
%   waterfall((domain),t,range);
%   hold on
% end;
% 
% figure(5)
% ylabel('Time','fontsize',15);
% xlabel('Sample space','fontsize',15);
% zlabel('Posterior density','fontsize',15);