clc;
clear all;
j=sqrt(-1);
x=300,y=100,z=100; %目標(biāo)位置 
degrade=pi/180; %角度轉(zhuǎn)換為弧度
l=340/100; %波長
r=0.5; %陣元離中心的距離
X=[0,200,50;0,0,100;0,0,0]; %各子陣的中心位置，每一列代表一個(gè)中心點(diǎn)的位置
Angle=zeros(2,3);
for i=1:3
Xtheta=asin(sqrt((x-X(1,i)).^2+(y-X(2,i)).^2)/sqrt((x-X(1,i)).^2+(y-X(2,i)).^2+(z-X(3,i)).^2)); 
Xfai=acos(abs(x-X(1,i))/sqrt((x-X(1,i)).^2+(y-X(2,i)).^2)) ;
Angle(1,i)=Xtheta;
Angle(2,i)=Xfai; %目標(biāo)相對各子陣中心的方位角和俯仰角，在后面為了驗(yàn)證程序正確與否，只用了第一陣
%即第一列數(shù)據(jù)進(jìn)行計(jì)算，是四元十字陣
end

fs=2000; %信號采樣頻率
t=0:1/fs:2;
%signal=2*sin(2*pi*17.5*t)+1.5*sin(2*pi*34*t)+sin(2*pi*53*t)+sin(2*pi*100*t);
signal=3*sin(2*pi*100*t);

N_x=1024; %信號長度
A=[exp(-j*2*pi*0.5*sin(Angle(1,1))*cos(Angle(2,1))/l),exp(-j*2*pi*0.5*sin(Angle(1,1))*cos(Angle(2,1)-pi/2)/l),exp(-j*2*pi*0.5*sin(Angle(1,1))*cos(Angle(2,1)-pi)/l), exp(-j*2*pi*0.5*sin(Angle(1,1))*cos(Angle(2,1)-pi*3/2)/l)];
signal=signal(:,1:N_x);
signal=A.'*signal+randn(4,N_x)+j*randn(4,N_x);
R=1/N_x*(signal*signal.');
%[V,D]=eig(R);
%[lambda,index] = sort((diag(D)));
%UU=V(:,index(1:3));
[U,S,V]=svd(R);
UU=U(:,2:4);

for theta=0:90
for fai=0:180
AA=[exp(-j*2*pi*r*sin(theta*degrade)*cos(fai*degrade)/l);
exp(-j*2*pi*r*sin(theta*degrade)*cos((fai-90)*degrade)/l);
exp(-j*2*pi*r*sin(theta*degrade)*cos((fai-180)*degrade)/1);
exp(-j*2*pi*r*sin(theta*degrade)*cos((fai-270)*degrade)/l)];
P=AA'*AA;
AAA=AA'*UU*UU'*AA;
Pmusic(theta+1,fai+1)=abs(P./AAA);
end
end

theta_x=0:90;
fai_x=0:180;
mesh(fai_x,theta_x,Pmusic);
title('MUSIC空間譜');
xlabel('方位角'); 
ylabel('俯仰角');
zlabel('角譜');
grid on; 
figure(2)
plot(fai_x,Pmusic)
xlabel('方位角fai');
ylabel('信號功率');
figure(3)
plot(theta_x,Pmusic);
xlabel('俯仰角theta');

Cmax=max(Pmusic);
Mmax=max(Cmax);
[X,Y]=find(Pmusic==Mmax);