bootstrap.asv

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

function [xSamples,q,Cr,mPre,xCr] = bootstrap(y,R,Q,initVar,numSamples,v,Cr,xCr);
% 功能 ： 对pfdemo.m中给定的模型进行粒子滤波：SIR算法 
%         通过误差校正来提高估计精度
% 输入 ：   - y = 观测值
%           - R = 测量噪声方差
%           - Q = 过程噪声方差
%           - initVar = 状态初始方差
%           - numSamples = 采样数目
% 输出 ：   - xSamples = 后验状态采样
%           - q = 归一化权值



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

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

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;

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

wPost = sum(xSamples.*q); 
wPre=sum(xPre.*q);
mPre=mean(xPre);
mPost=mean(xSamples);
for t=1:rows-1, 
    
 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); 

if Cr(t+1,1)>1.5
    if Cr(t+1,1)>2.5 && abs(Cr(t,1)-Cr(t+1,1))>1.5
%调节方法3  070202
       tempCr1=sqrt(((wPre(t+1).^(2))./20-y(t+1)).^2);
       
       xCr1=2*wPre(t+1)-wPost(t+1)*log;
       tempCr2=sqrt(((xCr(t+1,1).^2)./20-y(t+1)).^2);
       xCr2=2*wPost(t+1)-wPre(t+1);
       tempCr3=sqrt(((xCr(t+1,1).^2)./20-y(t+1)).^2);
       if tempCr1<tempCr2 && tempCr1< tempCr3
           xCr(t+1,1)=2*wPre(t+1)-wPost(t+1)*log; 
       
% 调节方法2 070202
%         tempCr1=((wPre(t+1).^(2))./20-y(t+1)).^2;
%         if Cr(t+1,1)>tempCr1
%             xCr(t+1,1)=2*wPre(t+1)-wPost(t+1);
%             tempCr2=((xCr(t+1,1).^2)./20-y(t+1)).^2;
%         else
%             xCr(t+1,1)=2*wPost(t+1)-wPre(t+1);
%             tempCr3=((xCr(t+1,1).^2)./20-y(t+1)).^2;
%         end;


% 调节方法1 070130-070202    
%       xPre(:,t+1) = predictstates(xSamples(:,t+1-1),t+1-1,Q);                    % 状态预测
%       q(:,t+1) = importanceweights(xPre(:,t+1),y(t+1,1),R,q(:,t+1));               % 权值计算
%       [xSamples(:,t+1)] = resampling(xPre(:,t+1),q(:,t+1)); 
%       q(:,t+1)=1/N*ones(N,1)
%       wPost(t+1) = sum(1/N*xSamples(:,t+1)); 
%       xCr(t+1,1)=wPost(t+1)
else
     xCr(t+1,1)=wPost(t+1);
    end;
    
else
    xCr(t+1,1)=wPost(t+1);
end;
end;


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