SRP室内语音定位

clc
clear all
close all
format compact;
%————-室内语音定位测试SRP效果————
y=audioread(‘locexample.wav’); %读入信号

wlen=4096; %窗长
step=2048; %步长
c=345; %声速
xyz=[-2 2 -3 3 0.5 0.5]; %search area xmin,xmax,ymin,ymax,zmin,zmax
gs=0.15; % spatial resolution
fs=44100;
Pbs=[0.01-2, 0.01-2, 0.01-2, 3.99-2, 3.99-2, 3.99-2;
0.01-3, 3.00-3, 5.99-3, 0.01-3, 3.00-3, 5.99-3;
0.50, 0.50, 0.50, 0.50, 0.50, 0.50];

%—————————————————
px=xyz(1)+gs/2:gs:xyz(2)-gs/2;
py=xyz(3)+gs/2:gs:xyz(4)-gs/2;
nx=length(px); ny=length(py);
Nm=size(Pbs,2);

% y=[zeros(fs/5,Nm);y];
sl=size(y,1); % signal length
nframes=floor((2*sl-wlen)/(2*step)); % the number of frames
awin=repmat(hanning(wlen),1,Nm); % window

%————————————-
%frame by frame processing
%————————————-
Tar_Es=zeros(nframes,2); %存储搜索得到的位置
Tar_Es2=zeros(nframes,2);
f=[fs/wlen*(0:wlen/2) fs/wlen*(-wlen/2+1:-1)]’;
f(7:end-6)=0; %一定要对频率限制，满足阵元间距小于半波长
for ni=1:nframes
sind=(ni-1)*step+1; % start window index
xn=y(sind:(sind+wlen-1),:).*awin; % get frame of input data
%—————————-
%GCC-PHAT
%———————————–
Y=fft(xn); % DFT of input signals
Y=exp(1j*angle(Y)); %避免分母为0
GCCmat=zeros(nx,ny); %配对
GCCmat2=zeros(nx,ny);
for i=1:nx
for j=1:ny
tau=sqrt((Pbs(1,:)-px(i)).^2+(Pbs(2,:)-py(j)).^2)/c;
Y3=sum(Y.*exp(1j*2*pi*f*tau),2);
% GCCmat(i,j)=norm(Y2,2);
GCCmat(i,j)=Y3’*Y3;
% ——麦克风频域配对——–
% temp=0;
% for k1=1:Nm
% for k2=1:Nm
% GCCmat2(i,j)=GCCmat2(i,j)+sum(Y(:,k1).conj(Y(:,k2)).*exp(1j*2*pi*f(tau(k1)-tau(k2))));
% end
% end
end
end
[ind_x,ind_y]=find(GCCmat==max(max(GCCmat)),1);
Tar_Es(ni,1:2)=[px(ind_x) py(ind_y)];
Tar_Es(ni,:)
% GCCmat2=abs(GCCmat2);
% [ind_x,ind_y]=find(GCCmat2==max(max(GCCmat2)),1);
% Tar_Es2(ni,1:2)=[px(ind_x) py(ind_y)];
% Tar_Es2(ni,:)
end