particlefilter.m

this is 3d tr model for airplane target tracking

function [W_k_1 , X_k_resampled , X_estimated ] = ParticleFilter (X_k_1 , z_k , N , Q , State_Number , Sigma_R , Sigma_B , Sigma_PH , T , W_k_1)

X_k = Model_CV (X_k_1 , T , Q) ;

X_k_ob = observe (X_k , N) ;

R_w = diag([ Sigma_R ,Sigma_B , Sigma_PH ]) ;

for i = 1 : N

    %W_k(i) = mvnpdf ( X_k_ob(:,i),z_k, R_w ) ;

    var_obmodelR = 1 / (2*pi*Sigma_R)^.5 * exp(-(z_k(1)-X_k_ob(1,i))^2/(2*Sigma_R)) ;

    var_obmodelB = (2*pi*Sigma_B)^-.5 * exp(-(z_k(2)-X_k_ob(2,i))^2/(2*Sigma_B)) ;

    var_obmodelPHI = 1/(2*pi*Sigma_PH)^.5 * exp(-(z_k(3)-X_k_ob(3,i))^2/(2*Sigma_PH)) ;

    W_k(i) = W_k_1(i) *  var_obmodelR * var_obmodelB * var_obmodelPHI ;
    
   % pause
   % clc
    %W_k(i) = W_k_1(i) * Gussian(z_k , X_k_ob(:,i) , R_w);
end

Iam_sum = sum(W_k);

% if Iam_sum < 10^-3
%
%     [X_estimated1 , X_estimated2 , X_estimated3] = sph2cart(z_k(3),z_k(2),z_k(1)) ;
%
%     X_estimated = [X_estimated1,X_estimated2,X_estimated3]';
%
%
%     X_estimated = [X_estimated ; X_k_1(4:9,1)];
%
%     X_k_resampled = kron (X_estimated,ones(1,N));
% else

W_k = W_k' / Iam_sum;

Neff = 1 / (sum(W_k.^2))

X_estimated = X_k * W_k ;

W_k_1 = W_k;

if Neff < N/3

    X_k_resampled = resample(X_k , W_k , N) ;

    W_k_1 =  1/N * ones(N,1);
    
else

    X_k_resampled = X_k ;
    
end

 plot3(X_k_ob(1,:),X_k_ob(2,:),W_k,'o')
 
%
  pause
return