my_predictstates3.m

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

function x_predict =my_predictstates3(F,x_samples,Q,numSamples);
% PURPOSE : Performs the prediction step of the sequential SIR algorithm for 
%         : the model described in the file sirdemo1.m.
% INPUTS  : - x = The state samples.
%           - t = The current time step.
%           - Q = The variance of the process noise.
% OUTPUTS : - x_predict = The state samples after the prediction step.

%  x_predict(:,:,t) = predictstates(samples(:,:,t),Q);

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

% D_r=10; D_alpha=0.0052;D_beta=0.0052;%   D_alpha=D_beta=0.3 Degree
% 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]; 
% R=[D_r^2 0 0;0 D_alpha^2 0;0 0 D_beta^2];
% initx = [100000 100000 30 20 80000]';
% %init_x_filter=[100200 100100 40 30 80010]';  % the inition of the filter. you can choose it .
% 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];
% numSamples=500;



x_predict=zeros(size(x_samples));

process_noise=sample_gaussian(zeros(length(Q),1),Q,numSamples)'; % process_noise is ss-S  5-500

x_predict=F*x_samples+process_noise;          % x is 5-500 Matrix