bootstrap.m

本人珍藏的粒子滤波源代码

function [X,q] = bootstrap(actualX,Z,R,Q,initVar,numSamples);%function:加入新函数
% PURPOSE : This m file performs the bootstrap algorithm (a.k.a. SIR,
%           particle filter, etc.) for the model specified in the
%           file sirdemo1.m. 
% INPUTS  : - actualx = The true hidden state. 
%           - y = The observation.
%           - R = The measurement noise variance parameter.
%           - Q = The process noise variance parameter.
%           - initVar = The initial variance of the state estimate.
%           - numSamples = The number of samples.
% OUTPUTS : - x = The estimated state samples.
%           - q = The normalised importance ratios.

% AUTHOR  : Nando de Freitas - Thanks for the acknowledgement :-)
% DATE    : 08-09-98

if nargin < 6, error('Not enough input arguments.'); end
[rows,cols,page] = size(actualX);                         % rows = Max number of time steps.4*1*N
S = numSamples;                                           % Number of samples;500
Nstd = 2;                                                 % No of standard deviations for error bars;
X=zeros(S,rows,page);                                     %zeros:构成全0矩阵，S行，rows列500*4*50
XU=zeros(S,rows,page);
q=zeros(S,rows,page);

% SAMPLE FROM THE PRIOR:
% =====================
X(:,:,1) = randn(S,4)*sqrt(initVar);   %x(:,1):?;sqrt:对矩阵的每一个元素分别开方
for i=1:4
mean(X(:,i,1));                        %mean:求矩阵各列平均值
cov(X(:,i,1));                         %cov:协方差矩阵    ？
end

% figure(1)%figure:创建图形窗
% clf;     %clf:清除当前图形窗
% subplot(221)
% plot(actualx)   %绘制二维坐标系
% ylabel('State x','fontsize',15);    %Y轴标注（适用于三维图形）
% xlabel('Time','fontsize',15);

% UPDATE AND PREDICTION STAGES:
% ============================
for t=1:page-1,
   
%   figure(1)        %figure(i):打开第i个图形
%   subplot(222)      %在平铺位置建立图形轴系  subplot(n,m,p):将图形窗口分为n*m个子图，在第p个子图处绘制图形
%   plot(y)          %plot(y):绘制线性直角坐标的二维图，以y中元素的下标作为X坐标，y中元素的值作为Y坐标   ，其中y是一个数组
%   ylabel('Output y','fontsize',15);
%   xlabel('Time','fontsize',15);
%   hold on        %保持
%   plot(t*ones(1,49),[-19:1:29],'r');      % r代表红色
%   hold off      %解除保持状态
%   subplot(223)
%   hold on
%   plot(t,mean(x(1:S,t,1)),'ro',t,actualx(t,1),'go');    % 绘制两条图线
%   hold on
%   errorbar(t,mean(x(1:S,t,1)),Nstd*std(x(1:S,t,1)),Nstd*std(x(1:S,t,1)),'k')
%   legend('Posterior mean estimate','True value');
%   ylabel('Sequential state estimate','fontsize',15)
%   xlabel('Time','fontsize',15)

  XU(:,:,t) = predictstates(X(:,:,t),t,Q);      %自建子函数   
  q(:,:,t+1) = importanceweights(XU(:,:,t),Z(2,1,t+1),R);    %自建子函数
  X(:,:,t+1) = updatestates(XU(:,:,t),q(:,:,t+1));
end;