sir_rmse.m

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

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 0 100150 0 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 


no_of_runs=4;
%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.

for M=1:no_of_runs
[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,initV,numSamples);  % output samples and q are 500-50 Matrix.  

% Posterior mean estimate
x_PF= mean(samples,2);       % x_PF is 3_Dim  ;

x_PF2=reshape(x_PF,ss,Total_time); % x_PF2 is 2_Dim

x_MonteC(:,:,M)=x;
y_MonteC(:,:,M)=y;

x_pf_MonteC(:,:,M)=x_PF2;
end;

%=============RMSE for PF===================

%position_error_rms
position_error_pf = x_MonteC([1 3],:,:) - x_pf_MonteC([1 3],:,:); % postion_error is a  2-T-M 3-DIM Matrix;
position_mean_error_pf=mean(position_error_pf.^2,3);% mean value of the matrix along the dimension 3.  postion_mean_error is a 2-T Matrix.
position_rms_pf=sqrt(sum(position_mean_error_pf)); % position_rms is a 1-T Matrix.


% xdot_rms
xdot_error_pf=x_MonteC([2],:,:)-x_pf_MonteC([2],:,:);%xdot_error is a 1-T-M Matrix;
xdot_rms_pf=sqrt(mean(xdot_error_pf.^2,3)); %xdot_rms is a 1-T Matrix;

%ydot_rms
ydot_error_pf=x_MonteC([4],:,:)-x_pf_MonteC([4],:,:);%ydot_error is a 1-T-M Matrix;
ydot_rms_pf=sqrt(mean(ydot_error_pf.^2,3)); %ydot_rms is a 1-T Matrix;

%=============Plot Resulut==============
figure(1)
plot(1:length(position_rms_pf),position_rms_pf,'r');
xlabel('T')
ylabel('RMS postion error')

figure(2)
plot(1:length(xdot_rms_pf),xdot_rms_pf,'r');
xlabel('T')
ylabel('RMS Vx error')

figure(3)
plot(1:length(ydot_rms_pf),ydot_rms_pf,'r');
xlabel('T')
ylabel('RMS Vy error')