sirdemo3.m

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

%程序说明:用matlab程序编写的一个三维雷达跟踪粒子滤波器.观测量为:Range ,Az ,El
%Target States 5 Dimension.[x xdot, y ydot ,z]

numSamples=800;        % Number of  samples per time step. 
D_r=80; D_alpha=0.0087;D_beta=0.0087;%   D_alpha=D_beta=0.5 Degree
Total_time =150;
T_step=1;
ss = 5;              %state size     X=[x,dot_x, y, dot_y, z]
os = 3;              %observation size   Y=[r ,alpha , beta]
F = [1 T_step 0 0 0; 0 1 0 0 0; 0 0 1 T_step 0; 0 0 0 1 0;0 0 0 0 1]; 
Q_CV=[T_step^3/3 T_step^2/2 0 0 0 ;T_step^2/2 T_step 0 0 0;0 0 T_step^3/3 T_step^2/2 0;0 0 T_step^2/2 T_step 0; 0 0 0 0 1/T_step];
q_CV=0.2; %Power Spectral density of the corresponding continuous process noise.  
Q=Q_CV.*q_CV;
R=[D_r^2 0 0;0 D_alpha^2 0;0 0 D_beta^2];
initx = [100000 30 100000 20 80000]';
initx_filter=[100150 1 100150 1 80050]';  %the initial of the filter. you can choose it .
initV =20*[10000 0 0 0 0 ;0 100 0 0 0 ;0 0 10000 0 0;0 0 0 100 0;0 0 0 0 100];%the initial Covariance of the filter 


%ss = 5; % state size     X=[x,y,dot_x, dot_y, z]
%os = 3; % observation size   Y=[r ,alpha , beta]
%samples=zeros(ss,numSamples,Total_time);      % samples=5-500-50;     
%q=zeros(1,numSamples,Total_time);             % q 1-500-50 Matrix.


[x,y] = my_ekf_sample(F, Q, R, initx, Total_time); %output x is ss-T matrix. y is os-T matrix.

%%%%%%%%%%%%%%%%%%%%%  Particle Filter   %%%%%%%%%%%%%%%

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

% Posterior mean estimate
x_PF= mean(samples,2);       % prediction 5-1-50;


%X_range=x_PF(1,:);
%Y_range=x_PF(2,:);
%X_velocity=x_PF(3,:);
%Y_velocity=x_PF(4,:);


% figure(1)
% clf;
% plot(1:length(x),x(1,:),1:length(x),X_range(:),'r')
% legend('True value','Posterior mean estimate');


figure(1)

subplot(221)

plot(1:length(x),x(1,:),1:length(x),x_PF(1,:),'r');
ylabel('Range in X axis','fontsize',15);
xlabel('Time');

subplot(222)
plot(1:length(x),x(3,:),1:length(x),x_PF(3,:),'r');
ylabel('Range in Y axis','fontsize',15);
xlabel('Time');

subplot(223)
plot(1:length(x),x(2,:),1:length(x),x_PF(2,:),'r');
ylabel('Velocity in X axis','fontsize',15);
xlabel('Time');

subplot(224)
plot(1:length(x),x(4,:),1:length(x),x_PF(4,:),'r');
ylabel('Velocity in Y axis','fontsize',15);
xlabel('Time');