车辆碰撞预判，matlab有错误，哪个大神可以改下？

% -----------------------------------------------------------------------------
% 安防多斜率 多目标配对
% n目标 n个斜率 +MTD
% fs=1e6;  % 1M
% 8倍抽取  Fs=125k
% #  
% # 缺陷：
% #       1.信号源是三角调制波平移和上下移得到，不是通过公式得到
% #       2.多目标配对写的方法较复杂，效果也不是很好
% #       3.无Mti MTD算法
% ------------------------------------------------------------------------------
clc;
clear all;
close all;

%%
B=250e6;
fs=500e6;  % 1M
dt=1/fs;
c=3e8;
fc=34.5e9;
n_MTI=4;
n_MTD=n_MTI;

T=[40e-3  ,50e-3   ,60e-3   ,70e-3 ];

n_slope=length(T);

R=[ 20  ,40    ,60    ,80    ];% 单位：m
V=[ 1   ,2     ,3     ,4     ];% 单位：m/s
n_target=length(R);

for i=1:n_target
    tao(i)=2*R(i)/c;
    n_tao(i)=fix(tao(i)/dt);  % 目标1的时间延迟
end

t_delay_max=1e-4;
n_delay_max=t_delay_max/dt+1;
t_total_1=t_delay_max;           % 15000m时间延迟
n_T_total=0;

f_add=zeros(n_slope,n_target);

for i=1:n_slope
    T_half(i)=T(i)/2;
    K(i)=B/T_half(i);
    eval(['t',num2str(i),'=0:dt:T(i);']);
    n_T(i)= T(i)/dt+1;
    n_T_half(i)= fix(n_T(i)/2);
    t_total_1=t_total_1+T(i);
    for j=1:n_target
        f_add(i,j)=(tao(j)-n_tao(j)*dt)*K(i);
    end
    n_T_total=n_T(i)+n_T_total;  % 三角波调制时间点数，不含延迟时间
end
% 怎么处理处理连续时间和离散时间的关系
t_total=0:dt:t_total_1+n_slope*dt;
n_t_total=n_delay_max+n_T_total;
% tao(1)=2*(R(1)+V(1)*t1)/c;
%% 发射
for i=1:n_slope
    eval(['Sm_T_up_',num2str(i),'=K(i)*t',num2str(i),'(1:n_T_half(i));']);
    eval(['Sm_T_down_',num2str(i),'=-K(i)*(t',num2str(i),'(n_T_half(i)+1:n_T(i))-T(i));']);
    eval(['Sm_T_',num2str(i),'=[Sm_T_up_',num2str(i),',Sm_T_down_',num2str(i),'];']);
end
Sm_T=zeros(1,n_t_total);
n_T_start=1;
n_T_end=0;
for i=1:n_slope  
    n_T_end=n_T_end+n_T(i);
    eval([ 'Sm_T(n_T_start:n_T_end)=Sm_T_',num2str(i),';' ]);
    n_T_start=n_T_start+n_T(i);

end

figure;
hold on;
plot(Sm_T);


% t = 0:ts_bb:T(1)+T(2)+T(3)+T(4);
% Sm_T = 2*pi*(fc + Sm_T).*(t - n_t_half*T_half) + 0;%　发射信号相位
%% 接收
Sm_R=zeros(n_target,n_t_total);
for i=1:n_target
    Sm_R(i,:)=[zeros(1,n_tao(i)),Sm_T(1:(n_t_total-n_tao(i)) )];
    f_IF(i)=2*K(1)*R(i)/c;  %目标1的距离频移
    f_d(i)=2*fc*V(i)/c;   %目标1的多普勒频移
end

for j=1:n_target
    n_***_t=n_tao(j);
    for i=1:n_slope
        %eval([]);
        Sm_R(j,n_***_t+1:n_***_t+n_T_half(i))=Sm_R(j,n_***_t+1:n_***_t+n_T_half(i))-f_add(i,j);
        Sm_R(j,n_***_t+n_T_half(i)+1:n_***_t+n_T(i))=Sm_R(j,n_***_t+n_T_half(i)+1:n_***_t+n_T(i))+f_add(i,j);
        % 因为n_tao1取整平移后，有量化损失，上面加减f_add就是补偿这种损失
        n_***_t=n_***_t+n_T(i);
    end
        Sm_R(j,:)=Sm_R(j,:)+f_d(j);
end

% figure;
plot(Sm_R(1,:),'r');
plot(Sm_R(2,:),'k');
%% 差拍
for j=1:n_target
    Sm_B(j,:)=Sm_T-Sm_R(j,:);
    Sm_B(j,:)=abs(Sm_B(j,:));
end

plot(Sm_B(1,:),'y');
plot(Sm_B(2,:),'g');
%axis([1,n_T(1)+n_tao(1),-5e6,5e6]);
hold off;
% n_Sm_B=length(Sm_B(1,:));
% n_t_total=(n_Sm_B-1)*dt;
% t_total=0:dt:n_t_total;
***_temp=zeros(n_target,n_t_total);
for j=1:n_target
    ***_temp(j,:)=cos(2*pi*Sm_B(j,:).*t_total);    %调制，产生已调信号
end

***=***_temp(1,:);
for i=1:n_target-1
    ***=***+***_temp(i+1,:);
end

figure;
plot(***);

***=***/max(***);   %信号归一化
***=awgn(***,10);

figure;
plot(***);

%%  FFT变换
N_fft=1024;
tao_max=t_delay_max;
% win_fft = window(@hamming,N_fft); % fft窗函数  hamming rectwin gausswin
% % 加窗，压制旁瓣，同时也会展宽通带
n_fft_t_start=fix(tao_max/dt) + 1;
take_up=zeros(n_slope,N_fft);
take_down=zeros(n_slope,N_fft);
***_fft_up=zeros(n_slope,n_MTI+1,N_fft);
***_fft_down=zeros(n_slope,n_MTI+1,N_fft);
% win_fft = window(@hamming,N_fft); % fft窗函数  hamming rectwin gausswin

% FFT数据点抽取
ext_number=8;
for i=1:n_slope   
    take_up(i,:)=***(n_fft_t_start+1:ext_number:n_fft_t_start+N_fft*ext_number);
    take_down(i,:)=***(n_fft_t_start+1+n_T_half(i):ext_number:n_fft_t_start+n_T_half(i)+N_fft*ext_number);
    n_fft_t_start=n_fft_t_start+n_T(i);
end

Fs=fs/ext_number;
freq=linspace(0,Fs/2,N_fft/2);


up_mat=zeros(4,1024);
down_mat=zeros(4,1024);
for i=1:n_slope
    up_mat(i,:)=abs(fft(take_up(i,:),1024));
    down_mat(i,:)=abs(fft(take_down(i,:),1024));
end
figure
hold on
plot(up_mat(3,:));
plot(down_mat(3,:),'r');
% axis([0 Fs/5 0 1000])
legend('1','2')
title('fft')
grid on
hold off

%% 恒虚警处理  

WIN=10;
PRO=2;              %%%  减少保护单元PRO，需要增加参考单元的数目
N_cfar=N_fft;
first =WIN+PRO+1;
last = N_cfar-WIN-PRO;

cfar_up=up_mat;
cfar_down=down_mat;

MEN_up= NaN*ones(n_slope,N_cfar);
MEN_down= NaN*ones(n_slope,N_cfar);
for i=1:n_slope  
    for k=1:first-1
        MEN_up(i,k)=sum(cfar_up(i,k+PRO+1:k+WIN+PRO))/WIN;      
    end
    for k=first:last
         % MEN(k) = mean([u(k-WIN-PRO:k-PRO-1);u(k+PRO+1:k+WIN+PRO)]);
         sum1=sum(cfar_up(i,k-WIN-PRO:k-PRO-1))/WIN;
         sum2=sum(cfar_up(i,k+PRO+1:k+WIN+PRO))/WIN;
         MEN_up(i,k)=(sum1+sum2)/2;
    end
    for k=last+1:N_cfar
        MEN_up(i,k)=sum(cfar_up(i,k-WIN-PRO:k-PRO-1))/WIN;     
    end

    for k=1:first-1
        MEN_down(i,k)=sum(cfar_down(i,k+PRO+1:k+WIN+PRO))/WIN;      
    end
    for k=first:last
         % MEN(k) = mean([u(k-WIN-PRO:k-PRO-1);u(k+PRO+1:k+WIN+PRO)]);
         sum1=sum(cfar_down(i,k-WIN-PRO:k-PRO-1))/WIN;
         sum2=sum(cfar_down(i,k+PRO+1:k+WIN+PRO))/WIN;
         MEN_down(i,k)=(sum1+sum2)/2;
    end
    for k=last+1:N_cfar
        MEN_down(i,k)=sum(cfar_down(i,k-WIN-PRO:k-PRO-1))/WIN;     
    end
end

%  输出高于门限值的距离单元和幅度值
pfa=10^(-3);  % 恒虚警概率
n2=2*WIN;
n3=-1/n2;
% alfa=n2*(pfa^n3-1);
% alfa=n2*(pfa^n3-1)/3.5;
alfa=2;
u_MEN_up=alfa*MEN_up;
u_MEN_down=alfa*MEN_down;

freq=linspace(-Fs/2,Fs/2,N_cfar);
% 画门限图
%{  %}
for i=1:n_slope  
    figure
    hold on
    plot(cfar_up(i,:),'*');
    plot(u_MEN_up(i,:),'r');
    %axis([0 Fs/5 0 1000])
    legend('up','CAFR门限')
    title(['up',num2str(i)])
    grid on
    hold off

    figure
    hold on
    plot(cfar_down(i,:),'*');
    plot(u_MEN_down(i,:),'r');
    %axis([0 Fs/5 0 1000])
    legend('down','CAFR门限')
    title(['down',num2str(i)])
    grid on
    hold off
end


%% 目标信息判决
% target_max=100;

HXJ_up =zeros(n_slope,N_cfar/2);
INDEX_up =zeros(n_slope,N_cfar/2);
INDEX_temp1=0;
INDEX_temp2=0;
count_1=zeros(n_slope,1);
count_2=zeros(n_slope,1);

HXJ_down =zeros(n_slope,N_cfar/2);
INDEX_down =zeros(n_slope,N_cfar/2);

for j=1:n_slope  
     for i=1:N_cfar/2    % 选出大于门限的点
        if (cfar_up(j,i)>u_MEN_up(j,i)) && (cfar_up(j,i)>60)
            count_1(j)=count_1(j)+1;
            HXJ_up(j,count_1(j))=cfar_up(j,i);
            INDEX_up(j,count_1(j))=i;           
        end
     end
     
     count_2(j)=0;
     for i=1:N_cfar/2    % 选出大于门限的点
        if (cfar_down(j,i)>u_MEN_down(j,i)) && (cfar_down(j,i)>60)
            count_2(j)=count_2(j)+1;
            HXJ_down(j,count_2(j))=cfar_down(j,i);
            INDEX_down(j,count_2(j))=i;            
        end
     end
end

count_1_max=max(count_1);
count_2_max=max(count_2);

UpDown1=zeros(n_slope,count_1_max+1);
UpDown2=zeros(n_slope,count_2_max+1);
Top1=zeros(n_slope,count_1_max);
Top2=zeros(n_slope,count_2_max);

for i=1:n_slope   %  上扫频　标出频点间是　断开　上升　下降　
   for j=1: count_1(i)-1
       if INDEX_up(i,j)==(INDEX_up(i,j+1)-1)
            if HXJ_up(i,j)>HXJ_up(i,j+1)
                UpDown1(i,j+1)=1; % 下降
            else
                UpDown1(i,j+1)=2; % 上升
            end
       end
   end
end
for i=1:n_slope  % 标出频点 是否为顶点
   for j=1: count_1(i)
       if UpDown1(i,j)==0 && UpDown1(i,j+1)==0
           Top1(i,j)=1; % 顶点
       elseif UpDown1(i,j)==0 && UpDown1(i,j+1)==1
           Top1(i,j)=1;
       elseif UpDown1(i,j)==0 && UpDown1(i,j+1)==2
           Top1(i,j)=2;
       elseif UpDown1(i,j)==1 && UpDown1(i,j+1)==0
           Top1(i,j)=2;
       elseif UpDown1(i,j)==1 && UpDown1(i,j+1)==1
           Top1(i,j)=2;
       elseif UpDown1(i,j)==1 && UpDown1(i,j+1)==2
           Top1(i,j)=2;
       elseif UpDown1(i,j)==2 && UpDown1(i,j+1)==0
           Top1(i,j)=1;
       elseif UpDown1(i,j)==2 && UpDown1(i,j+1)==1
           Top1(i,j)=1;
       elseif UpDown1(i,j)==2 && UpDown1(i,j+1)==2  
           Top1(i,j)=2;
       end
   end   
end  
for i=1:n_slope   % 去除不是顶点的点
    j=1;
    while j<=count_1(i)
       if Top1(i,j)==2
            for h=j:count_1(i)
                HXJ_up(i,h)=HXJ_up(i,h+1);
                INDEX_up(i,h)=INDEX_up(i,h+1);
                if h+1<=count_1(i)
                    Top1(i,h)=Top1(i,h+1);
                elseif h+1==count_1(i)+1
                    Top1(i,h)=0;                    
                end
            end
            count_1(i)=count_1(i)-1; % 减少一点
            j=j-1;
       end
       j=j+1;
    end
end


for i=1:n_slope   %　下扫频 标出频点间是　断开　上升　下降　
   for j=1: count_2(i)-1
       if INDEX_down(i,j)==(INDEX_down(i,j+1)-1)
            if HXJ_down(i,j)>HXJ_down(i,j+1)
                UpDown2(i,j+1)=1; % 下降
            else
                UpDown2(i,j+1)=2; % 上升
            end
       end
   end
end
for i=1:n_slope  % 标出频点 是否为顶点
   for j=1: count_2(i)
       if UpDown2(i,j)==0 && UpDown2(i,j+1)==0
           Top2(i,j)=1; % 顶点
       elseif UpDown2(i,j)==0 && UpDown2(i,j+1)==1
           Top2(i,j)=1;
       elseif UpDown2(i,j)==0 && UpDown2(i,j+1)==2
           Top2(i,j)=2;
       elseif UpDown2(i,j)==1 && UpDown2(i,j+1)==0
           Top2(i,j)=2;
       elseif UpDown2(i,j)==1 && UpDown2(i,j+1)==1
           Top2(i,j)=2;
       elseif UpDown2(i,j)==1 && UpDown2(i,j+1)==2
           Top2(i,j)=2;
       elseif UpDown2(i,j)==2 && UpDown2(i,j+1)==0
           Top2(i,j)=1;
       elseif UpDown2(i,j)==2 && UpDown2(i,j+1)==1
           Top2(i,j)=1;
       elseif UpDown2(i,j)==2 && UpDown2(i,j+1)==2  
           Top2(i,j)=2;
       end
   end   
end  
for i=1:n_slope   % 去除不是顶点的点
    j=1;
    while j<=count_2(i)  % 为什么不用for，因为在matlab中 for循环中不能改变循环变量i
       if Top2(i,j)==2
            for h=j:count_2(i)
                HXJ_down(i,h)=HXJ_down(i,h+1);
                INDEX_down(i,h)=INDEX_down(i,h+1);
                if h+1<=count_2(i)
                    Top2(i,h)=Top2(i,h+1);
                elseif h+1==count_2(i)+1
                    Top2(i,h)=0;                    
                end
            end
            count_2(i)=count_2(i)-1; % 减少一点   
            j=j-1;
       end
       j=j+1;
    end   
end

%% 计算R V
count_1_max=max(count_1);
count_2_max=max(count_2);
up_target_num_max=count_1_max;
down_target_num_max=count_2_max;
n_num=up_target_num_max*down_target_num_max;
updown_num=count_1.*count_2;
   
index_up=INDEX_up(1:n_slope,1:up_target_num_max);
index_down=INDEX_down(1:n_slope,1:down_target_num_max);

freq_up=Fs*(index_up-1 )/N_cfar;               %%索引指数：N_cfar/2-INDEX(1)+1；
freq_down=Fs*(index_down-1 )/N_cfar;

% 画RV图
R_plot=1:100;  
V_plot_up=zeros(n_num ,100,n_slope);
V_plot_down=zeros(n_num ,100,n_slope);
for j=1:n_slope  
    for i=1:count_1(j)
        for h=1:count_2(j)  
            V_plot_up((i-1)*count_2(j)+h,:,j)=( freq_up(j,i)*c -4*B*R_plot/T(j))/(2*fc);
            V_plot_down((i-1)*count_2(j)+h,:,j)=( -freq_down(j,h)*c +4*B*R_plot/T(j))/(2*fc);
        end
    end
end
figure
axis([0,100,-20,20])  
hold on
for j=1:n_slope  
    for i=1:updown_num(j)
        plot(R_plot,V_plot_up(i,:,j),'r');
        plot(R_plot,V_plot_down(i,:,j),'b');
    end
end
title('RV图')
xlabel('R,m')           %添加x轴标注
ylabel('V,m/s')         %添加y轴标注
grid on



R_get=zeros(n_slope,n_num );
V_get=zeros(n_slope,n_num );

for j=1:n_slope  % 计算出RV（含鬼目标）
    for i=1:count_1(j)
        for h=1:count_2(j)
            R_get(j,count_2(j)*(i-1)+h)=c*(freq_up(j,i)+freq_down(j,h))*T(j)/(8*B);
            V_get(j,count_2(j)*(i-1)+h)=c*(freq_up(j,i)-freq_down(j,h))/(4*fc);  
        end
    end
end
for j=1:n_slope
    plot(R_get(j,:),V_get(j,:),'og');
end
hold off


R_get_2=R_get;
V_get_2=V_get;

Axis_d=zeros(n_slope*n_num,n_num);  %距离
Axis_temp2=0;
Axis_temp3=0;

Axis_x=zeros(n_slope*n_slope*n_num,4);
n_count=0;
k=1;
for k=1:n_slope-1  % 去除鬼目标
    for j=1:updown_num(k)      
        for i=k+1:n_slope            
            for h=1:updown_num(i)
                if abs( V_get_2(k,j)-V_get_2(i,h) )<0.2
                    if abs(R_get_2(k,j)-R_get_2(i,h) )<0.5
                        n_count=n_count+1;
                        Axis_x(n_count,:)=[k,j,i,h];  % 将两个相关联的目标的数组位置存储起来                     
                        Axis_temp3=Axis_temp3+1;
                        Axis_temp2=Axis_temp2+1; % 找到对应目标，自+1
                        Axis_d(Axis_temp3,Axis_temp2)=( V_get_2(k,j)-V_get_2(i,h) )^2+( R_get_2(k,j)-R_get_2(i,h) )^2;                       
                    end
                end
            end
            if Axis_temp2>1  %检验,如果对应的点数大于1，就删除多余的点
                [min_d,min_I]=min(Axis_d);
                Axis_x(n_count-Axis_temp2+1,:)=Axis_x(n_count-Axis_temp2+1+min_I,:);
                n_count=n_count-Axis_temp2+1;
            end
            Axis_temp2=0;
        end
    end
end

Target=zeros(n_count,2,n_count);
Target_temp=zeros(n_count,1); % 某个目标对应的 结果数
n_Target=0;  % 总共有多少个目标
n_temp1=0;
n_temp2=0;  %标志位，
n_temp3=0;  %标志位
n_temp4=0;  %第n个目标
n_temp5=0;  %第n个目标
j=0;

n_RV=0;
n_count_temp1=0;
for i=1:n_count  % 将同一目标的RV平均
    if i==1
        n_Target=n_Target+1;     

        Target(1,:,n_Target)=Axis_x(1,1:2);
        Target(2,:,n_Target)=Axis_x(1,3:4);
        Target_temp(n_Target)=2;
    else
        for j=1:n_Target
            for h=1:Target_temp(j)
                if Axis_x(i,1:2)==Target(h,:,j)
                    n_temp2=1;
                    n_temp4=j;
                end
                if Axis_x(i,3:4)==Target(h,:,j)
                    n_temp3=1;
                    n_temp5=j;
                end
            end
           
        end
        if n_temp2==1 && n_temp3==0
            Target_temp(n_temp4)=Target_temp(n_temp4)+1;
            Target(Target_temp(n_temp4),:,n_temp4)=Axis_x(i,3:4);            
        elseif n_temp2==0 && n_temp3==1
            Target_temp(n_temp5)=Target_temp(n_temp5)+1;
            Target(Target_temp(n_temp5),:,n_temp5)=Axis_x(i,1:2);                        
        elseif n_temp2==0 && n_temp3==0
            n_Target=n_Target+1;
            Target(1,:,n_Target)=Axis_x(i,1:2);
            Target(2,:,n_Target)=Axis_x(i,3:4);
            Target_temp(n_Target)=2;
        end
        n_temp2=0;
        n_temp3=0;   
        n_temp4=0;
        n_temp5=0;         
    end
end

R_get_3=zeros(1,n_Target);
V_get_3=zeros(1,n_Target);

for i=1:n_Target
    for j=1:Target_temp(i)
        A=R_get_2(Target(j,1,i),Target(j,2,i));
        R_get_3(i)=R_get_3(i)+R_get_2(Target(j,1,i),Target(j,2,i));
        V_get_3(i)=V_get_3(i)+V_get_2(Target(j,1,i),Target(j,2,i));
    end
    R_get_3(i)=R_get_3(i)/Target_temp(i);
    V_get_3(i)=V_get_3(i)/Target_temp(i);
end

index_up
index_down
freq_up
freq_down
% R_get;
% V_get;

R_get_2
V_get_2

R_get_3
V_get_3