%波達方向矩陣法
%2008.04.07
%矢量水聽器
clc
clear
c=1500;f=1e3;lamd=c/f;
fs=5*f;                                          %采樣頻率為信號頻率的5倍
Ln=1:200;                                    %200拍
theta=pi/10
fi=pi/4                        %信號入射的方位角、俯仰角
ss=exp(j*2*pi*f/fs*Ln);                 %信號
u1=[1 sin(fi)*cos(theta) sin(fi)*sin(theta) cos(fi)].';%陣列流形 
Ap0=kron(ss,u1);
x=Ap0(:,1:100);y=Ap0(:,2:101);

Rxx=x*x'/length(Ln);                     %
Ryx=y*x'/length(Ln);
[D,V]=eig(Rxx);
vi=D(:,4);
Rxx0=vi*vi'/1;
R=Ryx*Rxx0;
[M,N]=eig(R);

u=M(:,1);
Ru=((u(2,1).^2+u(3,1).^2+u(4,1).^2)).^0.5;
uk=u(2,1)/Ru;
vk=u(3,1)/Ru;
wk=u(4,1)/Ru;
fi1=asin((uk.^2+vk.^2).^0.5)
theta1=angle(uk+j*vk)