一个BEAMFORMING 用于声源定位的程序，帮忙看看错在哪啊？

djh713

编了一段BEAMFORMING的程序，基于时延累加的，但结果总是不对，看了好久也不知什么地方错了，各位高手帮忙看下吧。多谢了

clear all
clc
A_x=0.1;%阵列面x方向网格的间距
A_y=0.1;%阵列面y方向网格的间距
A_z=1;%传声器阵列所在平面的z坐标
%--------------------阵列-7×7平面阵列-----------------------
Position_A=[-3*A_x,3*A_y,A_z;    -2*A_x,3*A_y,A_z;    -A_x,3*A_y,A_z;    0,3*A_y,A_z;    A_x,3*A_y,A_z;    2*A_x,3*A_y,A_z;    3*A_x,3*A_y,A_z;
    -3*A_x,2*A_y,A_z;    -2*A_x,2*A_y,A_z;    -A_x,2*A_y,A_z;    0,2*A_y,A_z;    A_x,2*A_y,A_z;    2*A_x,2*A_y,A_z;    3*A_x,2*A_y,A_z;
    -3*A_x,A_y,A_z;    -2*A_x,A_y,A_z;    -A_x,A_y,A_z;    0,A_y,A_z;    A_x,A_y,A_z;    2*A_x,A_y,A_z;    3*A_x,A_y,A_z;
    -3*A_x,0,A_z;    -2*A_x,0,A_z;    -A_x,0,A_z;    0,0,A_z;    A_x,0,A_z;    2*A_x,0,A_z;    3*A_x,0,A_z;
    -3*A_x,-A_y,A_z;    -2*A_x,-A_y,A_z;    -A_x,-A_y,A_z;    0,-A_y,A_z;    A_x,-A_y,A_z;    2*A_x,-A_y,A_z;    3*A_x,-A_y,A_z;
    -3*A_x,-2*A_y,A_z;    -2*A_x,-2*A_y,A_z;    -A_x,-2*A_y,A_z;    0,-2*A_y,A_z;    A_x,-2*A_y,A_z;    2*A_x,-2*A_y,A_z;    3*A_x,-2*A_y,A_z;
    -3*A_x,-3*A_y,A_z;    -2*A_x,-3*A_y,A_z;    -A_x,-3*A_y,A_z;    0,-3*A_y,A_z;    A_x,-3*A_y,A_z;    2*A_x,-3*A_y,A_z;    3*A_x,-3*A_y,A_z];
L=49;


Ax=Position_A(:,1);Ay=Position_A(:,2);Az=Position_A(:,3);%阵列坐标
ax=reshape(Ax,sqrt(L),sqrt(L));
ay=reshape(Ay,sqrt(L),sqrt(L));
az=reshape(Az,sqrt(L),sqrt(L));

%-----------------------定义声源坐标------------------------------------
S_x=0.1;%阵列面x方向网格的间距
S_y=0.1;%阵列面y方向网格的间距
S_z0=0;%传声器阵列所在平面的z坐标
Position_S=[-4*S_x,4*S_y,S_z0;    -3*S_x,4*S_y,S_z0;    -2*S_x,4*S_y,S_z0;    -S_x,4*S_y,S_z0;    0,4*S_y,S_z0;    S_x,4*S_y,S_z0;    2*S_x,4*S_y,S_z0;    3*S_x,4*S_y,S_z0;    4*S_x,4*S_y,S_z0;
     -4*S_x,3*S_y,S_z0;    -3*S_x,3*S_y,S_z0;    -2*S_x,3*S_y,S_z0;    -S_x,3*S_y,S_z0;    0,3*S_y,S_z0;    S_x,3*S_y,S_z0;    2*S_x,3*S_y,S_z0;    3*S_x,3*S_y,S_z0;    4*S_x,3*S_y,S_z0;
     -4*S_x,2*S_y,S_z0;    -3*S_x,2*S_y,S_z0;    -2*S_x,2*S_y,S_z0;    -S_x,2*S_y,S_z0;    0,2*S_y,S_z0;    S_x,2*S_y,S_z0;    2*S_x,2*S_y,S_z0;    3*S_x,2*S_y,S_z0;    4*S_x,2*S_y,S_z0;
     -4*S_x,S_y,S_z0;    -3*S_x,S_y,S_z0;    -2*S_x,S_y,S_z0;    -S_x,S_y,S_z0;    0,S_y,S_z0;    S_x,S_y,S_z0;    2*S_x,S_y,S_z0;    3*S_x,S_y,S_z0;    4*S_x,S_y,S_z0;
     -4*S_x,0,S_z0;    -3*S_x,0,S_z0;    -2*S_x,0,S_z0;    -S_x,0,S_z0;    0,0,S_z0;    S_x,0,S_z0;    2*S_x,0,S_z0;    3*S_x,0,S_z0;    4*S_x,0,S_z0
     -4*S_x,-S_y,S_z0;    -3*S_x,-S_y,S_z0;    -2*S_x,-S_y,S_z0;    -S_x,-S_y,S_z0;    0,-S_y,S_z0;    S_x,-S_y,S_z0;    2*S_x,-S_y,S_z0;    3*S_x,-S_y,S_z0;    4*S_x,-S_y,S_z0;
     -4*S_x,-2*S_y,S_z0;    -3*S_x,-2*S_y,S_z0;    -2*S_x,-2*S_y,S_z0;    -S_x,-2*S_y,S_z0;    0,-2*S_y,S_z0;    S_x,-2*S_y,S_z0;    2*S_x,-2*S_y,S_z0;    3*S_x,-2*S_y,S_z0;    4*S_x,-2*S_y,S_z0;
     -4*S_x,-3*S_y,S_z0;    -3*S_x,-3*S_y,S_z0;    -2*S_x,-3*S_y,S_z0;    -S_x,-3*S_y,S_z0;    0,-3*S_y,S_z0;    S_x,-3*S_y,S_z0;    2*S_x,-3*S_y,S_z0;    3*S_x,-3*S_y,S_z0;    4*S_x,-3*S_y,S_z0;
     -4*S_x,-4*S_y,S_z0;    -3*S_x,-4*S_y,S_z0;    -2*S_x,-4*S_y,S_z0;    -S_x,-4*S_y,S_z0;    0,-4*S_y,S_z0;    S_x,-4*S_y,S_z0;    2*S_x,-4*S_y,S_z0;    3*S_x,-4*S_y,S_z0;    4*S_x,-4*S_y,S_z0];

Sx=Position_S(:,1);Sy=Position_S(:,2);Sz=Position_S(:,3);%阵列坐标
N=81;
%------------------定义声源---------------------------------------
f=2000;%定义声场的频率
k=2*pi*f/344;%对应频率下的声场波数
c=344; %声速
rho=1.25; %空气密度
r0=0.001;%脉动球源半径
Ua=1; %脉动球源振速
de=1;% 重建距离constant parameter
omega=2*pi*f;
A=(rho*c*k*r0*r0)*Ua*(k*r0+i)/(1+k*k*r0*r0);
S=zeros(N,1);
% S(76)=1;%定义声源位置为（0,0.1，0）
S(31)=A;
% S(18)=1;
SS=reshape(S,sqrt(N),sqrt(N));%在7×7方阵上重构声源
sx=reshape(Sx,sqrt(N),sqrt(N));
sy=reshape(Sy,sqrt(N),sqrt(N));%将Sx与Sy重构为7×7方阵
sz=reshape(Sz,sqrt(N),sqrt(N));
ssx=sx(:,1);
ssy=sy(1,:)';%提取x与y为7个向量
%mesh(sx,sy,SS);
figure(1)
contourf(ssx,ssy,SS);
colorbar;

%--------------------确定声源到阵列面上各传声器间的距离-------------------------------------
for i=1:N;
    for j=1:L;
        r(i,j)=sqrt((Sx(i)-Ax(j))^2+(Sy(i)-Ay(j))^2+(Sz(i)-Az(j))^2);
    end
end
%-------------------------------以下为计算阵列面接收信号----------------------------------
clear i j
for m=1:N;
    for n=1:L;
%         h(m,n)=exp(j*(2*pi*f*r(m,n)/c-k*r(m,n)))./r(m,n);   
        h(m,n)=1/r(m,n)*exp(i*(omega*r(m,n)/c-k*r(m,n)));
    end
end
pa=S.'*h;
Pa=reshape(pa,sqrt(L),sqrt(L));
ax=reshape(Ax,sqrt(L),sqrt(L));
ay=reshape(Ay,sqrt(L),sqrt(L));
az=reshape(Az,sqrt(L),sqrt(L));
figure(2)
mesh(ax,ay,abs(Pa).');
figure(3)
contourf(ax,ay,abs(Pa).');
colorbar;
clear m n
%---------------------------重建声源面------------------------------------------
%-------------------确定声源面大小及节点坐标-----------------------------------
kj=1.5;%重建面孔径为 1.5×1.5m，保证阵列开角在30度范围内
d=0.1; %重建面节点间距0.1m
M=kj/d+1;%每个方向的网格节点数
for l=1:M
    X_s(l,1)=-kj/2+(l-1)*d;%声源面网格在X向的坐标
end
xs=repmat(X_s,1,M);%扩展为M×1的列阵，X的坐标为按行依次排列，如1 2 3 1 2 3 1 2 3
for g=1:M
    Y_s(g,1)=kj/2-(g-1)*d;%声源面网格在Y向的坐标
end
ys=repmat(Y_s',M,1);%扩展为M×1的列阵，Y的坐标为同一值重复X次，如1 1 1 2 2 2 3 3 3
zs=repmat(0,M,M);%声源面网格在Z向的坐标
%------------------------确定阵列点对重建面上每一聚焦点的距离------------------------------------------


%------------------------以阵列中心点为参考点，确定其余各阵列点相对于该点的时间延迟---------------------
RR=repmat(A_z,L,M^2);
for m=1:L;
    for n=1:M^2;
        R(m,n)=sqrt((xs(n)-Ax(m))^2+(ys(n)-Ay(m))^2+(zs(n)-Az(m))^2);
        DR(m,n)=R(m,n)-RR(m,n);
        T(m,n)=DR(m,n)./c;%-------------------求延时
        H(m,n)=exp(-j*(omega*T(m,n)-k*DR(m,n)))*R(m,n);
    end
end
%-----------------------------------根据已知的延时对声源面进行聚焦------------------------
Ps=pa*H./max(max(H));

ps=reshape(Ps,M,M);
sx=reshape(xs,M,M);
sy=reshape(ys,M,M);
figure(4)
mesh(sx,sy,abs(ps).');
figure(5)
contourf(sx,sy,abs(ps).');
colorbar;

[ 本帖最后由 djh713 于 2008-7-20 12:42 编辑 ]