bootstrap.m

对运动声目标进行航迹跟踪

function [x,q] = bootstrap(Ns,R,Q,initVar,numSamples);
% 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 < 5, error('Not enough input arguments.'); end

rows=Ns;     % rows = Max number of time steps.
S = numSamples;             % Number of samples;

x=zeros(S,3);
xu=zeros(S,3);
q=zeros(S,1);

% SAMPLE FROM THE PRIOR:
% =====================
x(1,:) = sqrt(initVar)*randn(1,S);
x(2,:) = sqrt(initVar)*randn(1,S);
x(3,:) = sqrt(initVar)*randn(1,S);
% mean(x(:,1));
% cov(x(:,1));

% figure(1)
% clf;

% UPDATE AND PREDICTION STAGES:
% ============================
for t=1:rows-1,
   
  xu = predictstates(x,Q);
 
  q = importanceweights(Xs_fft,id_v,xu);
  
  x = updatestates((xu(1,:),q);
  
end;

plot(t,mean(x(1:S,t,1)),'ro');