mimo_main.m

这是一个关于多输入多输出系统的频域忙识别系统的matlab源码

   %%% Get Ry1(w)
   if w2_equal_minus_w3_plus_arfa & ~Using_Bispectrum
      for w=1:NF
         Ry1(:,:,w) = V(:,:,mod(w+arfa_w2_plus_w3-1,NF)+1)*Rx1(:,:,w)*V(:,:,w);
      end
   else
      for w=1:NF
         Ry1(:,:,w) = V(:,:,mod(NF+1-w, NF)+1)*Rx1(:,:,w)*V(:,:,mod(sum_w1_w2+NF+1-w, NF)+1)';
      end
   end
   
   
   %%% Calculate Ryn(w) and Cyn, Cyn=Ryn'*Ryn.
   if w2_equal_minus_w3_plus_arfa & ~Using_Bispectrum
      for w=1:NF  %% Cyn is a "m x m x Freq_number x NF" matrix, the last m is the index ii
         for Freq_Index=1:Freq_number*m %% Cyn=Ryn^{H} x Ryn
            Ryn_dummy = V(:,:,mod(w+arfa_w2_plus_w3-1,NF)+1)*Rxn(:,:,w,Freq_Index)*V(:,:,w);
            Ryn(:,:,Freq_Index,w) = Ryn_dummy;
            Cyn(:,:,Freq_Index,w) = Ryn_dummy'*Ryn_dummy;
         end
      end
   else
      for w=1:NF  %% Cyn is a "m x m x Freq_number x NF" matrix, the last m is the index ii
         for Freq_Index=1:Freq_number %% Cyn=Ryn^{H} x Ryn
            for ii=0:m-1
               Ryn_dummy = V(:,:,mod(NF+1-w, NF)+1)*Rxn(:,:,w,Freq_Index+Freq_number*ii)*V(:,:,mod(Ref_Frequencies(Freq_Index)+1-w, NF)+1)';
               Ryn(:,:,Freq_Index+Freq_number*ii,w) = Ryn_dummy;
               Cyn(:,:,Freq_Index+Freq_number*ii,w) = Ryn_dummy*Ryn_dummy';
            end
         end
      end
   end
   
   
   
   %%% Calculate the W(w) using SVD of one matrix Ry1.
   for w=1:NF
      
      [Udummy,Sdummy,Vdummy] = svd(Ry1(:,:,w));
      
      Smat(:,w)=real(diag(Sdummy));
      
      [Smat_sort(:,w),Smat_Index]=sort(abs(Smat(:,w)));
      
      W(:,:,mod(1-w, NF)+1)=Udummy(:,Smat_Index);    %% The orthogonal matrix W(w)
   end
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%% The following is Cardoso's Joint Diagonalization
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%% 
   TSTART = clock;
   
   Selected_Freq_number=floor(Freq_number*m*Freq_Select_Ratio);
   D_closeness=zeros(Freq_number*m,1);
   for w=1:1  %% Joint Diagonalization of Cyn(:,:,:,w)
      
      [Udummy,Ddummy] = joint_diag(reshape(Cyn(:,:,:,w),m,m*m*Freq_number),0.00000001);
      
      Ddummy=reshape(Ddummy,m,m,Freq_number*m);
      
      for ii=1:Freq_number*m
         D_diag(:,ii)=sort(diag(abs(Ddummy(:,:,ii))));
         
         D_closeness(ii)=(D_diag(m,ii)-D_diag(1,ii))/D_diag(m,ii);
      end
      
      for w=1:Freq_number
         D_diag(:,w)=sort((abs(HH(1,:,w)))).';
         D_diag(:,w+Freq_number)=sort((abs(HH(2,:,w)))).';
      end
      
      [D_close_dummy Selected_w2]=sort(-D_closeness);
      
      Selected_w2=Selected_w2(1:Selected_Freq_number);
      
      
      if iloop==1
         
         Dominant_freq=Selected_w2(1);
         
         %%% Calculate H_order
         Rx1_index_d=ceil(Dominant_freq/Freq_number);
         if Less_Input_than_Output
            [H_dummy H_order_i]=sort(reshape(abs(H(Dominant_freq-(Rx1_index_d-1)*NF/2,Rx1_index_d,1:n)),n,1));
            
            H_order(H_order_i)=m-n+1:m;
            H_order(n+1:m)=1:m-n;
         else  %% m=n, INPUT=OUTPUT
            [H_dummy H_order_i]=sort(reshape(abs(H(Dominant_freq-(Rx1_index_d-1)*NF/2,Rx1_index_d,1:n)),n,1));
            
            H_order(H_order_i)=1:n;
            H_order(n+1:m)=n+1:m;
         end   
         
      else
         if ~length(find(Selected_w2 == Dominant_freq))
            Selected_w2=[Dominant_freq Selected_w2.'].';
         end
      end
      
      Dominant_freq_position=find(Selected_w2 == Dominant_freq);
      if Dominant_freq_position ~= 1
         Selected_w2(2:Dominant_freq_position)=Selected_w2(1:Dominant_freq_position-1);
         Selected_w2(1)=Dominant_freq;
      end
      
      
   end
      
      
   for w=1:NF  %% Joint Diagonalization of Cyn(:,:,:,w)
      
      [Udummy,Ddummy] = joint_diag(reshape(Cyn(:,:,Selected_w2,w),m,m*length(Selected_w2)),0.00000001);
      
      Ddummy=reshape(Ddummy,m,m,length(Selected_w2));
      
      D1(:,w)=abs(diag(Ddummy(:,:,1)));
      
      [D1_sort(:,w),D1_Index]=sort(abs(D1(:,w)));
      
      Wn(:,:,mod(1-w, NF)+1)=Udummy(:,D1_Index);    %% The joint orthogonal matrix W(w)
      
   end
   
   TIME_Joint_Daig = etime(clock,TSTART)
   
   
   
   H_abs=reshape(abs(H(w2,Rx1_index,:)),m,1)*abs(GAMMA)
   Smat_mean=mean(Smat_sort,2)
   Smat_std=std(Smat_sort,0,2)
   Smat_mean_array=diag(Smat_mean)*ones(m,NF);
   Smat_1_std_array=diag(Smat_mean+Smat_std_plus_coeff*Smat_std)*ones(m,NF);
   Smat__1_std_array=diag(Smat_mean-Smat_std_minus_coeff*Smat_std)*ones(m,NF);
   
   if Select_Freq_by_SVD
      EIG_INDEX=find( (~(Smat_sort(m,:)<Smat__1_std_array(m,:)|Smat_sort(1,:)>Smat_1_std_array(1,:))) );
   else
      EIG_INDEX=1:NF;
   end
   
   
   EIG_ONE=zeros(1,NF);EIG_ONE(EIG_INDEX)=1;
   if Select_Freq_by_Rx_cond
      EIG_INDEX=find(EIG_ONE(:) & Rx_INDEX(:))';
   end
   
   %%% Plot the Singular vlues at all frequncies.
   figure(1);clf;hold on;grid;
   plot(Smat_sort');
   plot(Smat_mean_array');
   plot(Smat__1_std_array','.');
   plot(EIG_INDEX,Smat_mean(m),'r*');
   title('Singular values at all frequencies');
   %axis([1 NF 0 10])   

   
   eig_index_his(EIG_INDEX,iloop)=1;
   Smat_his(:,:,iloop)=Smat;
   Smat_mean_his(:,iloop)=Smat_mean;
   Smat_std_his(:,iloop)=Smat_std;
   
   if Using_Joint_Diag
      W=Wn;
   end
   
   Hest = zeros(m,m,NF);
   for w=1:NF
      M = V_1(:,:,w) * W(:,:,w);
      Hest(:,:,w) = conj(M);    %%% Estimation of the system transfer function
   end
   
   
   if SYSTEM_REAL
      Hest(:,:,NF/2+2:NF)=conj(Hest(:,:,NF/2:-1:2));
   end
   
   Phase_hat=angle(Hest);       %%% the estimated phase with phase ambiguity
   
   TSTART = clock;
   hest=zeros(size(h));
   if CHOOSE_EIG
      for ii=1:m
         for jj=1:m  %% Reconstruct the minimum phase impulse response
            hest_dummy = real(rec_frommag_complex(abs(reshape(Hest(ii,H_order(jj),:),1,NF)),EIG_INDEX,L+L_extend));
            hest(1:L+L_extend,ii,H_order(jj)) = hest_dummy;
         end
      end
   else
      for ii=1:m
         for jj=1:m
            hest_dummy = real(rec_frommag_complex(abs(reshape(Hest(ii,H_order(jj),:),1,NF)),1:NF,L+L_extend));
            hest(1:L+L_extend,ii,H_order(jj)) = hest_dummy;
         end
      end
   end
   TIME_Mag_Reconstruction = etime(clock,TSTART)

   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
%%% Phase Retrieval Begin here, using the special matrix A
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
   
   Phase_matrix_1=zeros(m,m,NF);
   Phase_matrix_2=zeros(m,m,NF);
   for w=1:NF
      Phase_matrix_1(:,:,w)=inv(conj(Hest(:,:,w)))*reshape(B_x_phase(w,i_phase_index_1,:,:),m,m)...
         *pinv(Hest(:,:,mod(w+k_arfa-1,NF)+1).');
      Phase_matrix_2(:,:,w)=inv(conj(Hest(:,:,w)))*reshape(B_x_phase(w,i_phase_index_2,:,:),m,m)...
         *pinv(Hest(:,:,mod(w+k_arfa-1,NF)+1).');
   end
   
   Psi_1=zeros(m,NF);
   Psi_2=zeros(m,NF);
   for ii=1:m
      Psi_1(ii,:)=reshape(angle(Phase_matrix_1(ii,ii,:)),1,NF);
      Psi_2(ii,:)=reshape(angle(Phase_matrix_2(ii,ii,:)),1,NF);
   end
   
   Phase_1=reshape(Phase_true(i_phase_index_1,:,k_arfa+1),m,1);
   Phase_2=reshape(Phase_true(i_phase_index_2,:,k_arfa+1),m,1);
   
   Phase_1_sum=sum(Psi_1,2);
   Phase_2_sum=sum(Psi_2,2);
   
   Linear_phase_1=(Phase_1_sum(H_order)+Phase_1*NF)/pi
   Linear_phase_2=(Phase_2_sum(H_order)+Phase_2*NF)/pi
   
   Phase_1;
   Phase_1_est=-Phase_1_sum/NF;
   Phase_2;
   Phase_2_est=-Phase_2_sum/NF;
   
   Phi_1=zeros(m,NF);
   Phi_2=zeros(m,NF);
   
   A=hosmatrix(NF, k_arfa);
   A1=inv(A);
   for ii=1:m
      Phi_1(ii,2:NF) = (A1*reshape(Psi_1(ii,2:NF),NF-1,1)+Phase_1_est(ii)*sum(A1,2)).';
      Phi_2(ii,2:NF) = (A1*reshape(Psi_2(ii,2:NF),NF-1,1)+Phase_2_est(ii)*sum(A1,2)).';
   end
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
   %%% J. C. Pesquet's method for phase estimation
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
   
   Lamda_d=0.003;
   Lamda_a=0.003;
   
   PSI_1=fft(Psi_1,NF,2);
   PSI_2=fft(Psi_2,NF,2);
   
   Exp_k_arfa_term=ones(m,1)*(exp(-i*2*pi*k_arfa*(0:NF-1)/NF)-1);
   Exp_1_term=ones(m,1)*(exp(i*2*pi*(0:NF-1)/NF)-1);
   
   PHI_1 = (Exp_k_arfa_term.*PSI_1) ./ (abs(Exp_k_arfa_term).^2 + Lamda_d*abs(Exp_1_term).^2 + Lamda_a);
   PHI_2 = (Exp_k_arfa_term.*PSI_2) ./ (abs(Exp_k_arfa_term).^2 + Lamda_d*abs(Exp_1_term).^2 + Lamda_a);
   
   PHI_1(:,1)=zeros(m,1);
   PHI_2(:,1)=zeros(m,1);
   
   Phi_1_ifft=real(ifft(PHI_1,NF,2));
   Phi_2_ifft=real(ifft(PHI_2,NF,2));
   
   if Using_Pesquet_phase
      figure(201);clf
      for ii=1:m
         subplot(m,2,ii*2-1);
         plot(real(PHI_1(ii,:)));
         grid;
         title(sprintf('Real part of PHI_1(%d)',ii));
         
         subplot(m,2,ii*2);
         plot(imag(PHI_1(ii,:)));
         grid;
         title(sprintf('Imag part of PHI_1(%d)',ii));
      end
      
      figure(202);clf
      for ii=1:m
         subplot(m,2,ii*2-1);
         plot(real(PHI_2(ii,:)));
         grid;
         title(sprintf('Real part of PHI_2(%d)',ii));
         
         subplot(m,2,ii*2);
         plot(imag(PHI_2(ii,:)));
         grid;
         title(sprintf('Imag part of PHI_2(%d)',ii));
      end
      
      figure(203);clf
      for ii=1:m
         subplot(m,1,ii);hold on;
         plot(real(Phi_1_ifft(ii,:)),'b-');
         plot(real(Phi_1(ii,:)),'r:');
         grid;
         title(sprintf('Phi_1(%d)',ii));
      end
      
      figure(204);clf
      for ii=1:m
         subplot(m,1,ii);hold on;
         plot(real(Phi_2_ifft(ii,:)),'b-');
         plot(real(Phi_2(ii,:)),'r:');
         grid;
         title(sprintf('Phi_2(%d)',ii));
      end
   end
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
   %%% J. C. Pesquet's Phase estimation method ENDS here
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
   
   if Using_Pesquet_phase
      Phi_1=Phi_1_ifft;
      Phi_2=Phi_2_ifft;
   end
   
   %%%%%%%%%%%%%%
   %               Pay Attention here
   %%%%%%%%%%%%%%
   for w=1:NF
      for ii=1:m
         Phase_est_1(:,ii,w)=Phase_hat(:,ii,w)+Phi_1(ii,w)*ones(m,1);
         Phase_est_2(:,ii,w)=Phase_hat(:,ii,w)+Phi_2(ii,w)*ones(m,1);
      end
   end
   
   %% We can use either Phase_est_1 or Phase_est_2
   Phase_est=Phase_est_1;
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
   if Modify_Hest_by_Phase_est
      Hest=abs(Hest).*exp(i*Phase_est);
   end
   
   
   if Modify_Hest_Mag_by_Order_Constrain
      for (ii=1:m)
         for (jj=1:m)
            Hest(ii,jj,:) = abs(fft(hest(:,ii,jj),NF,1)).*exp(i*reshape(angle(Hest(ii,jj,:)),NF,1));
         end
      end
   end
      
   if PLOT_PHASE
      figure(2); clf;   %%% Plot the phase
      for ii=1:m
         for jj=1:n
            subplot(m,n,(ii-1)*n+jj);
            title(sprintf('Phase Est',ii,jj));grid;hold on;
            plot(reshape((Phase_true(ii,jj,:)),NF,1)/pi,'r:');
            plot(reshape((Phase_est(ii,H_order(jj),:)),NF,1)/pi,'b-');
            ylabel('Phase in PI');
            Current_axis=axis;
            Current_axis(1)=1;
            Current_axis(2)=NF;
            axis(Current_axis);
         end
      end
   end  %%% End of if PLOT_PHASE
   
   for ii=1:m
      for jj=1:m
         Phase_est_unwrap(ii,jj,:)=unwrap(Phase_est(ii,jj,:));
         Phase_true_unwrap(ii,jj,:)=unwrap(Phase_true(ii,jj,:));
         Phase_est_1_unwrap(ii,jj,:)=unwrap(Phase_est_1(ii,jj,:));
         Phase_est_2_unwrap(ii,jj,:)=unwrap(Phase_est_2(ii,jj,:));
      end
   end
   
   Phase_est_unwrap_his(:,:,:,iloop)=Phase_est_unwrap;
   
   for ii=1:m