%This file compares the performance between the proposed method, method of
%[15], and traditional MIMO radar in terms of RMSE.
clc
clear
tic
M_t = 10;                                              % Total number of transmitting antennas
M_r = 10;                                              % Total number of receiving antennas
d_t = 0.5;% transmitter spacings in wavelength
shift = 0;
K = 2;
Theta = [-9 -8];
Theta_rad = Theta*pi/180;                               % Directions of targets to the broadside of the array
L_tar = length(Theta);
A_t = exp(j*d_t*2*pi*(0:M_t-1)'*sin(Theta_rad));        % Uplink steering vectors
d_r=[ 0; 0.088751705948312; 0.521021743058417; 1.250017937612549 ; 1.824886967042458; 3.175813189953003;
    3.692431862918508; 7.318498475733820; 8.042845778221803; 9.000000000000000]'; %receive array antenna positions

A_r = exp(j*pi*d_r'*sin(Theta_rad));                     % Downlink steering vectors; Tx and Rx are assumed to be close to each other
T = 50;                                                  % number of radar pulses (slow-time)
t1=(-15+shift)/180*pi;
t2=(0+shift)/180*pi;
t3=(0+shift)/180*pi;
t4=(15+shift)/180*pi;

% ---------- Orthogonal waveforms ------%
N = 1024;                                               % number of smaples within one radar pulse (fast-time)
phi = [];
for kk = 1:M_t
    phi = [phi; (1/sqrt(N))*exp(j*2*pi*(kk/N)*(0:N-1))];
end
phi_beam = phi(1:4,:);
phi_beam_cvx = phi(1:2,:);
s_mimo = A_t.'*phi;

%------------ load beamspace matrices-----%
load Xopt;
MATTT2 = WW;
C = trace(MATTT2'*MATTT2);
MATTT2 = (MATTT2/sqrt(C))*sqrt(10);

load W_n_opt;
MATTT = W_n_opt;
C = trace(MATTT'*MATTT);
MATTT = (MATTT/sqrt(C))*sqrt(10);
A_beam = (MATTT)'*A_t
s_beam = sqrt(1)*A_beam.'*phi_beam;

load Wconvex
C3 = trace(Wconvex'*Wconvex);
Wconvex = Wconvex/sqrt(C3)*sqrt(10);
A_beam_cvx = Wconvex'*A_t;
s_beam_cvx = sqrt(1)*A_beam_cvx.'*phi_beam_cvx;

P_noise = 1;
SNR = [-30:3:30];
MSE = zeros(length(SNR), 3);
montcarlo = 500;
for itr = 1:montcarlo, itr
    %----------Generation of Signal Noise-----------------------%
    Beta = sqrt(0.5)*(randn(L_tar,T)+1i*randn(L_tar,T)); %reflection coefficient
    Z_new = []; %initialize noise vectors
    Z_mimo = [];    
    Z_new_cvx = [];
    
    for n_pulse = 1:T;
        z = sqrt(P_noise/2)*(randn(M_r,N)+1i*randn(M_r,N));
        
        z2 = z*conj(phi_beam.');
        Z_new = [Z_new reshape(z2,4*M_r,1)];
        
        z1 = z*conj(phi.');
        Z_mimo = [Z_mimo reshape(z1,M_t*M_r,1)];  
        
        
        z3 = z*conj(phi_beam_cvx.');
        Z_new_cvx = [Z_new_cvx reshape(z3,2*M_r,1)];          
    end
    
    %------------------- Noise Vectors ---------------------%
    Var_new = diag(1./sqrt(var(Z_new.')));
    Z_new = Var_new*Z_new;
    
    Var_mimo = diag(1./sqrt(var(Z_mimo.')));
    Z_mimo = Var_mimo*Z_mimo;
           
    Var_cvx = diag(1./sqrt(var(Z_new_cvx.')));
    Z_new_cvx = Var_cvx*Z_new_cvx;
    
    
    for snr = 1:length(SNR)
        P_tar = 10^(SNR(snr)/10);
        Beta_tar = sqrt(P_tar)*Beta;
        x_beam = [];
        x_mimo = [];
        x_beam_cvx = [];
        
        for n_pulse = 1:T;
            % compute signals observed at targets
            s_tar_beam = diag(Beta_tar(:,n_pulse))*s_beam; %signal at target for proposed method
            x_tar_beam = A_r*s_tar_beam; %signal at receive array for proposed method          
            
            s_tar_mimo = diag(Beta_tar(:,n_pulse))*s_mimo; %signal at target for MIMO
            x_tar_mimo = A_r*s_tar_mimo; %signal at receive array for MIMO          
            
            s_tar_beam_cvx = diag(Beta_tar(:,n_pulse))*s_beam_cvx; %signal at target for method of 15
            x_tar_beam_cvx = A_r*s_tar_beam_cvx; %signal at receive array for method of 15
            
            x_beam = [x_beam reshape(x_tar_beam*conj(phi_beam.'), 4*M_r, 1)];
            x_mimo = [x_mimo reshape(x_tar_mimo*conj(phi.'), M_t*M_r, 1)];
            x_beam_cvx = [x_beam_cvx reshape(x_tar_beam_cvx*conj(phi_beam_cvx.'), 2*M_r, 1)];
        end
        
        x_mimo = x_mimo + Z_mimo;
        R_mimo = (1/T)*x_mimo*x_mimo'; %cross-correlation matrix for MIMO
        
        x_beam_cvx = x_beam_cvx +Z_new_cvx;
        y_beam_cvx = [x_beam_cvx(1:(K-1)*M_t, :); x_beam_cvx(M_t+1:K*M_t, :)];
        R_cvx = (1/T)*y_beam_cvx*y_beam_cvx'; %cross-correlation matrix for method of [15]

        x_beam = x_beam +Z_new;
        [mm,nn]=size(x_beam);
        y_beam1=x_beam(1:10,:)+x_beam(11:20,:);
        y_beam2=x_beam(21:30,:)+ x_beam(31:40,:);
        y_beam=[y_beam1;y_beam2];
        R_beam = (1/T)*y_beam*y_beam';%cross-correlation matrix for proposed method      
        
        %discon_find finds a discontinuity in the phase profile
        %corresponding to a given beamspace matrix if it exists. unwrapkon.m, which is
        %called by angle_extract uses this information to either unwrap the
        %phase profile or not. The returned phase profile is then used in
        %comparison with the phase argument from ESPRIT to provide the DOA
        %estimates
        [T1,min1] = discon_find(t1,t2,MATTT);
        [T2,min2] = discon_find(t3,t4,MATTT);
        
        [TT1,min_cvx_1] = discon_find(t1,t2,Wconvex);
        [TT2,min_cvx_2] = discon_find(t3,t4,Wconvex);       
        
        
        psi = LS_ESPRIT(R_beam, L_tar, 0); %ESPRIT
        %target could be in 2 possible places (left, or right side of the sector). 
        %We have two targets, we divide our sector in two, therefore there are 4 possible target
        %arrangements which will be parsed using MUSIC.
        angles1(1)=angle_extract(angle(psi(1)),t1,t2,10,10,MATTT,T1,min1);
        angles1(2)=angle_extract(angle(psi(1)),t3,t4,10,10,MATTT,T2,min2);

        angles2(1)=angle_extract(angle(psi(2)),t3,t4,10,10,MATTT,T2,min2);
        angles2(2)=angle_extract(angle(psi(2)),t1,t2,10,10,MATTT,T1,min1);
        
        psi = LS_ESPRIT(R_cvx, L_tar, 0); %ESPRIT
        angles_cvx_1(1)=angle_extract(angle(psi(1)),t1,t2,10,10,Wconvex,TT1,min_cvx_1);
        angles_cvx_1(2)=angle_extract(angle(psi(1)),t3,t4,10,10,Wconvex,TT2,min_cvx_2);

        angles_cvx_2(1)=angle_extract(angle(psi(2)),t3,t4,10,10,Wconvex,TT2,min_cvx_2);
        angles_cvx_2(2)=angle_extract(angle(psi(2)),t1,t2,10,10,Wconvex,TT1,min_cvx_1);
        
        %-------------Parsing target arrangements using MUSIC------------%
        [A,B]=eig(R_mimo);
        G=A(:,3:100);
        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles1(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles1(1)/180*pi));
        v=kron(A_T,A_R);
        S1= real(v'*G*G'*v);

        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles1(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles1(2)/180*pi));
        v=kron(A_T,A_R);
        S2= real(v'*G*G'*v);
        
        if S1 <= S2
                    angles(1) = angles1(1);
                else
                    angles(1) = angles1(2);
        end


        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles2(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles2(1)/180*pi));
        v = kron(A_T,A_R);
        S1 = real(v'*G*G'*v);

        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles2(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles2(2)/180*pi));
        v=kron(A_T,A_R);
        S2= real(v'*G*G'*v);
        
                if S1 <= S2
                    angles(2) = angles2(1);
                else
                    angles(2) = angles2(2);
                end

        [A_cvx,B_cvx]=eig(R_mimo);
        G=A_cvx(:,3:100);
        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_1(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_1(1)/180*pi));
        v = kron(A_T,A_R);
        S1 = real(v'*G*G'*v);


        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_1(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_1(2)/180*pi));
        v = kron(A_T,A_R);
        S2 = real(v'*G*G'*v);
        
                if S1 <= S2
                    angles_cvx(1) = angles_cvx_1(1);
                else
                    angles_cvx(1) = angles_cvx_1(2);
                end


        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_2(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_2(1)/180*pi));
        v = kron(A_T,A_R);
        S1 = real(v'*G*G'*v);

        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_2(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_2(2)/180*pi));
        v=kron(A_T,A_R);
        S2= real(v'*G*G'*v);
        
                if S1 <= S2
                    angles_cvx(2) = angles_cvx_2(1);
                else
                    angles_cvx(2) = angles_cvx_2(2);
                end
        %------------Display Angle Estimates and Calculate RMSE----------%       
        DOA_beam = sort(angles);
        MSE(snr, 1) = MSE(snr, 1) + mean((Theta(:) - DOA_beam(:)).^2);
        %
        psi = LS_ESPRIT(R_mimo, L_tar, 1);
        DOA = asin((-1/(d_t*2*pi))*angle(psi))*180/pi;
        DOA_mimo = sort(DOA);
        
        MSE(snr, 2) = MSE(snr, 2) + mean((Theta(:) - DOA_mimo(:)).^2);
       
        DOA_cvx = sort(angles_cvx);
       
        MSE(snr, 3) = MSE(snr, 3) + mean((Theta(:) - DOA_cvx(:)).^2);
               
        disp([DOA_beam DOA_mimo' DOA_cvx])
    end
end
CM_cvx = Wconvex;
CM = MATTT;
crb_beam = crb(A_t, A_r, Theta_rad, CM, SNR, T, d_r, d_t, M_t, M_r);
crb_beam_cvx = crb(A_t, A_r, Theta_rad, CM_cvx, SNR, T, d_r, d_t, M_t, M_r);
crb_mimo = crb(A_t, A_r, Theta_rad, eye(M_t), SNR, T, d_r, d_t, M_t, M_r);
crb_beam_nonuni = crb(A_t, A_r, Theta_rad, MATTT2, SNR, T, d_r, d_t, M_t, M_r);
SNR, RMSE = sqrt(MSE/montcarlo)
figure
semilogy(SNR,RMSE(:,2),'-sqm', SNR,RMSE(:,3),'v-r', SNR,RMSE(:,1),'o-b','LineWidth',2,'MarkerSize',10),grid
hold on
xlabel('SNR (dB)')
ylabel('RMSE (Degrees)')
legend('Traditional MIMO radar', 'Method of [15]', 'Best Achievable Tx Beamspace')


save Example3_Workspace_RMSE.mat
