扩展互质阵列 csm算法 运行结果有问题，想看看哪里需要改 thanks

clear;
clc;
fl=2000;
fh=5000;
Ns=124;    %采样点数
J=31;   % 频率点数
K=3;
SNR=10;
d=30000;
c=300000000;
azi=[-30,40,60];%方向角
ULA_ratio1=3;   %互质阵列1            
 ULA_ratio2=5;   %互质阵列2
M1=ULA_ratio2;       %扩展互质阵列
M2=2*ULA_ratio1;   
M=M1+M2-1;     %阵元数
indexA=sort(unique([(0:1:M1-1)*ULA_ratio1 (0:1:M2-1)*ULA_ratio2]));%虚拟阵列
for i=1:length(indexA)         
     R_a(i,:)=indexA(i)-indexA(:);
end
R_b=unique(R_a); %去冗余后的虚拟阵列
MM=2*ULA_ratio1*ULA_ratio2+1;
%宽带信号
i_=sqrt(-1);
Vs0=10^(SNR/10);
Vs=sqrt(Vs0/2);
s=Vs*(randn(K,Ns)+i_*randn(K,Ns));
S=zeros(K,Ns);
for i=1:K    
  S(i,:)=fft(s(i,:));%时域信号变换至频域
end
m=1;
for i=1:J    
   Z(:,:,i)=S(:,m:m+3);    
  m=m+4;
end
%噪声信号
Vn0=ones(1,M);
Vn=diag(sqrt(Vn0/2));
n=Vn*(randn(M,Ns)+i_*randn(M,Ns));%噪声
N=zeros(M,Ns);
for i=1:M    
    N(i,:)=fft(n(i,:));%时域信号变换至频域
end
m=1;
for i=1:J    
   N1(:,:,i)=N(:,m:m+3);    
  m=m+4;
end
%信号模型
 f=linspace(fl,fh,J);%将频率等分J份
A=zeros(M,K);
mu=zeros(31,1);
azi1=[-28,-22,38,41,58];%方向角的估计值
sj=zeros(length(azi1));
sum=0;k=1;
for i=1:J     
   A1=exp(i_*2*pi*f(i)/c*[0:MM-1]'*d*sin(azi1*pi/180));    
   A=exp(-1*sqrt(-1)*2*pi*indexA'*d*f(i)*sin(azi*pi/180)/c);   %互质阵下的阵列流型    
   X=A*Z(:,:,i)+N1(:,:,i);    
  R(:,:,i)=X*X'/4; %协方差矩阵    
   z1(:,:,i)=reshape(R(:,:,i),100,1); %向量化    
%-ULA_ratio1*ULA_ratio2*d:d:ULA_ratio1*ULA_ratio2*d    
  r1=-ULA_ratio1*ULA_ratio2:1:ULA_ratio1*ULA_ratio2;    
 AA=[];    
    for ii=1:MM       
      aA=find(R_a==r1(ii));       
      AA=[AA;aA(1)];    
    end   
   z(:,:,i)=z1(AA,:,i);  %虚拟阵下的接收信号  
    Rx(:,:,i)=z(:,:,i)*z(:,:,i)'; %协方差矩阵    
    temp=eig(Rx(:,:,i));                                        
     temp=sort(temp);     
  for ii=1:MM-K        
     sum=sum+temp(ii);    
   end     
 P(:,:,i)= Rx(:,:,i)-sum/(MM-K)*eye(MM,MM);  
  [U,W1,V]=svd(P(:,:,i));     %奇异值分解   
   mu=mu+diag(W1);    
  sj=sj+inv(A1'*A1)*A1'*P(:,:,i)*A1*inv(A1'*A1);    
   [Q1(:,:,i),B(:,:,i)]=eig(P(:,:,i));  %特征分解，构造聚焦矩阵    
   Q0(:,k:k+MM-1)=Q1(:,:,i);    
   k=k+MM;
end
%中心频率的选择  
s0=1/J*sj;  
mu=1/J*mu;  
 for k1=1:J            %选择参考频率fc    
  A0=exp(i_*2*pi*f(k1)/c*[0:MM-1]'*d*sin(azi1*pi/180));    
  P0=A0*s0*A0';    
  [Up Dp Vp]=svd(P0);    
   Dp0=diag(Dp);    
  SUM=0;    
   for kk=1:K        
   SUM=SUM+(Dp0(kk)-mu(kk)).^2;    
  end    
  Result(k1)=SUM;  
end   
 [h,j]=min(Result);   
Q_=Q0(1+(j-1)*MM*MM:j*MM*MM);    
Q=reshape(Q_,MM,MM);   
Ry=zeros(MM,MM);
for i=1:J    
T(:,:,i)=Q*Q1(:,:,i)';  %聚焦矩阵   
 Ry=Ry+(T(:,:,i)*z(:,:,i))*(T(:,:,i)*z(:,:,i))';            %聚焦变换后的协方差矩阵              
  end  
Ry=Ry/J; 
[V,D]=eig(Ry); 
[EVA,I]=sort(diag(D));  
V=V(:,I);
 noise_subspace=V(:,1:MM-K);  
q=1; 
for angle=-90:1:90      
ang=[];    
   for m1=1:MM      
ang=[ang;exp(-i_*2*pi*f(j)*d*(m1-1)*sin(angle*pi/180)/c)];     
  end      
  p(q)=1/(ang'*noise_subspace*noise_substhankspace'*ang);          
q=q+1; 
enq
pm=10*log10(abs(p/max(p)));
thetaesti=-90:1:90;
plot(thetaesti,pm);

@EstelleYuan 谢谢