my_bootstrap3.m

用matlab程序编写的一个三维雷达跟踪粒子滤波器

function [samples,q] = my_bootstrap3(F,actualx,y,R,Q,initx,initV,numSamples)  % output samples and q are 500-50 Matrix.

% 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.         5-T matrix
%           - y = The observation.                     3-T Matrix
%           - 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.     500
%            - samples = The estimated state samples.
%           - q = The normalised importance ratios.

% [samples,q] = my_bootstrap3(x,y,R,Q,initV,numSamples);  % output samples and q are 500-50 Matrix.  

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

[ss,Total_time] = size(actualx);      % ss =5,    T=Total_time=50

%F = [1 0 1 0 0; 0 1 0 1 0; 0 0 1 0 0; 0 0 0 1 0;0 0 0 0 1]; 
%samples=zeros(ss,numSamples,Total_time);   % samples=5-500-50;
%xu=zeros(ss,numSamples,Total_time);        
%q=zeros(1,numSamples,Total_time);         % q 1-500-50 Matrix.
%Total_time = 50;
%initx = [100000 100000 30 20 80000]';
%initV =[400 0 0 0 0 ;0 400 0 0 0 ;0 0 16 0 0;0 0 0 16 0;400 0 0 0 0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5

% SAMPLE FROM THE PRIOR:

samples(:,:,1)=sample_gaussian(initx,initV,numSamples)';   % 5-500

% UPDATE AND PREDICTION STAGES:
% ============================
for t=1:Total_time-1             % T=50;  also the total time.
   
  xu(:,:,t) =my_predictstates3(F,samples(:,:,t),Q,numSamples);    
  q(:,:,t+1) =my_importanceweights3(xu(:,:,t),y(:,t+1),R);    
  samples(:,:,t+1) =my_updatestates3(xu(:,:,t),q(:,:,t+1));      %perform  the resampling step
end;