clc;
clear;
close all;
a=13.610e-3;
b=10.612e-3;
unnamed4(:,1) = [-a,-a,a,a];
unnamed4(:,2) = [-b,b,-b,b];
unnamed4(:,3) = [0,0,0,0];
x = unnamed4(:,1);
y = unnamed4(:,2);
z = unnamed4(:,3);

c = physconst('lightspeed');
f0 = 26e9;
lamda = c/f0;
N_x = 2^10;                  %快拍点数
iwave = 1;
st=randn(iwave,N_x);

%%

theta = [30];
phi = [20];
load("Rxx.mat","Rx");
[U,S] = eig(Rx);
[~,index] = sort(diag(S));
for i=1:size(Rx,1)
    U(:,i) = U(:,i)/sqrt(U(1,i)^2+U(2,i)^2+U(3,i)^2+U(4,i)^2); % 做归一化
end

Un = U(:,index(1:(length(x)-iwave)));
Us = U(:,index(length(x)-iwave+1):end);
Gn = Un*Un';

dir=0:1:89;
ang=0:1:89;
Pmusic = zeros(length(dir),length(ang));
for i=1:length(dir)
    for k=1:length(ang)
        a_tao = x*cosd(dir(i))*sind(ang(k))+y*sind(dir(i))*sind(ang(k))+z*cosd(ang(k));
        a_theta = exp(-1j*2*pi*a_tao/lamda);
        abs((a_theta)'*Gn*a_theta)
        Pmusic(i,k)=1./abs((a_theta)'*Gn*a_theta);
    end
end
figure
mesh(Pmusic);

P_music = 10*log10(Pmusic/min(min(Pmusic)));