tbd_viterbi.m

用于雷达微弱目标检测的TDB iterbi算法

clear all;
close all;
clc;
% function [ output_args ] = TBD_Viterbi( input_args )
%TBD_VITERBI Summary of this function goes here
%   Detailed explanation goes here

% This Code is to Perform the TBD algorithm based on the Viterbi Alogorithm


Total=10;
T_step=2.5;
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; % Power Spectral density of the corresponding continuous process noise.  
Q=Q_CV.*q_CV;

%============================================================
%============================================================
% for M=1:no_of_runs, % Monte Carlo Loop

  rand('state',sum(100*clock));   % Shuffle the pack!
  randn('state',sum(100*clock));   % Shuffle the pack!
  
x= zeros(ss,Total);
y= zeros(os,Total);     % For Range,Doppler Velocity and Bearing. 
processNoise =sample_gaussian(zeros(length(Q),1),Q,Total)';% Note ' here! state_noise_samples is: ss-Total_time
% measureNoise =sample_gaussian(zeros(length(R),1),R,Total';% observer_noise_samples is os-Total_time
%%%%====Calculate The initial Covariance  And State===============
Mach=340;
initx = [200000 -1.5*Mach 300000 -1.6*Mach 20000]';
x(:,1) =initx;                          % Initial state.
for t=2:Total
  x(:,t)=F*x(:,t-1)+processNoise(:,t);  % For CV Model. 
end;  
%  y=hfun(x,Totaln,T_step);  % To Calculate the Range, Doppler Velocity and Bearing.

y(1,:)=sqrt(x(1,:).^2+x(3,:).^2+x(5,:).^2);  % Calculate Range
y(2,:)=-1./y(1,:).*(x(1,:).*x(2,:)+x(3,:).*x(4,:));  % Calculate Doppler Velocity.
y(3,:)=180/pi*atan(x(3,:)./x(1,:));                
%%%%%%%%%%%%%%%%

% Total_time=Total=30;
Num_Frame=Total;
Num_Cell_x=256;  %Number of  Range Cells, We Choose the furthest Range Cells
Num_Cell_y=64; % the number of Doppler Velocity cells
SNR=10;
Power_noise_av=1;
Power_target_av=10^(SNR/10)*Power_noise_av;  % The average of Target Power.

Power_target=Power_target_av*ones(1,Total); % Generate No-fluctuating Target 
% Power_target=random('exp',Power_target_av,[1,Total]);  % Generate Exponentially distributed target power according to Swerling I model.
% Power_target=ones(1,Total); 
delta_x=100;  %Range Cell size
delta_y=1/64*3*Mach;  % the size of Doppler Velocity cells,decided by the maximum Doppler Velocity Range.  15.93m/s

c_blur=1;
Blur_x=c_blur*delta_x;  %  The blur effect of the Point Spread Function In Range direction.
Blur_y=c_blur*delta_y;  %  The blur effect in Doppler Direction.

%%  Choose the Furthest Range Cells
Furthest_range_cell=max(y(1,:))/delta_x+20; % We add 20 to avoid the target appears on the edge of a R-D map
Starting_range_cell=floor(Furthest_range_cell-Num_Cell_x);

%%
Data_target=zeros(Num_Cell_x,Num_Cell_y,Total); % Matrix representing the received power intensity in the R-D map. 

for t=1:Total
%    c(j)=((j-0.5)*delta_y-y(2,t)).^2/Blur_y^2  ;
         for i=1:Num_Cell_x
             for j=1:Num_Cell_y
                 
%                  c_t(i,j,t)=-((i+Starting_range_cell-0.5)*delta_x-y(1,t))^2/(Blur_x^2) - ((j-0.5)*delta_y-y(2,t))^2/(Blur_y^2);
%                  H(i,j,t)=Power_target(t)*exp(c_t(i,j,t));
        Data_target(i,j,t)=Power_target(t)*exp(-((i+Starting_range_cell-0.5)*delta_x-y(1,t))^2/(Blur_x^2) - ((j-0.5)*delta_y-y(2,t))^2/(Blur_y^2) );
             end                           
         end
end
Data_noise=random('exp',Power_noise_av,[Num_Cell_x,Num_Cell_y,Total]);


Data_scan=Data_target+Data_noise;  

  Data_target_max_add=0;
for i=1:6
    Data_target_max_add=max(max(Data_target(:,:,i)))+Data_target_max_add;
end
    

% 

v_max=3*340;
q_range=1;%
q_doppler=1;%
Data_integrate=zeros(Num_Cell_x,Num_Cell_y,Total);
RangeCell_previous_scan=zeros(Num_Cell_x,Num_Cell_y,Total);
Data_integrate(:,:,1)=Data_scan(:,:,1);
Total=10;
for k=2:Total
    for i=1:Num_Cell_x-25
        for j=2:Num_Cell_y-3
%             if (Data_scan(i,j,k)>0)
            R_temp=(i-0.5+Starting_range_cell)*delta_x;
            V_temp=(j-0.5)*delta_y;
            R_pre=(  R_temp^2+v_max^2*T_step^2+2*T_step*V_temp*R_temp  )^(1/2);
            RangeCell_previous_scan=ceil(R_pre/delta_x)-Starting_range_cell;
            DopplerCell_previous_scan=ceil(V_temp/delta_y);
            Data_integrateSource=Data_integrate(RangeCell_previous_scan-q_range:RangeCell_previous_scan+q_range,...
                                            DopplerCell_previous_scan-q_doppler:DopplerCell_previous_scan+q_doppler,k-1);
            
            Data_integrate(i,j,k)= Data_scan(i,j,k)+max((max(Data_integrateSource)));
%             end
        end
    end
end


% 
figure(101)
mesh(Data_integrate(:,:,6))
xlabel('多谱勒单元');ylabel('距离单元');zlabel('功率强度');title('累积矩阵');
axis([1,64,1,256,0,100]);
colormap([.5 .5 .5])
colormap (gray)

x=Data_integrate(2:220,2:50,6);
x=reshape(x,1,size(x,1)*size(x,2));

num=100;
maxdat=max(x);
mindat=min(x);
NN=hist(x,num);
xpdf=num*NN/(sum(NN)*(maxdat-mindat));
xaxis=mindat:(maxdat-mindat)/num:maxdat-(maxdat-mindat)/num;

figure(8);
plot(xaxis,xpdf)
b_esti=sqrt(var(x)*6/pi/pi)-1.1
a_esti=-1*b_esti*log( length(x)/sum(exp(x/b_esti)))-1.5

% a_esti=17.611;
% b_esti=2.34;

th_val_gbl_estimate=1/b_esti*exp((xaxis-a_esti)/b_esti).*exp(-1*exp( (xaxis-a_esti)/b_esti ));%%%估计出的Gumbel分布概率密度函数曲线
hold on
plot(xaxis,th_val_gbl_estimate,'r');
hold off