close all

clc

clear all

c=340;

fs=11025;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rm=[5 3 3];

c=340;

mic2=[0 1.0  2];     %第二个麦克风

src=[3 2.5 2];

d=1;

mic3=[0 1+d 2];  %第三个麦克风

mic1=[0 1-d  2];  % 第一个麦克风

t1=sqrt((src(1)-mic1(1))^2+(src(2)-mic1(2))^2)/c;                          %信号源与第一个麦克风之间的距离除以声速，得到传播时间

t2=sqrt((src(1)-mic2(1))^2+(src(2)-mic2(2))^2)/c;                          %信号源与第二个麦克风之间的距离除以声速，得到传播时间

t3=sqrt((src(1)-mic3(1))^2+(src(2)-mic3(2))^2)/c;                          %信号源与第三个麦克风之间的距离除以声速，得到传播时间

t12=t1-t2;

t32=t2-t3;

rp=((2*(d^2)-(c^2)*((t12)^2+(t32)^2)))/(2*c*(t12-t32))    %实际距离

for i=1:9

tt12(i)=t12;

tt32(i)=t32;

rpp(i)=rp;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

r=0.1;

n=8;

h=rir(fs, mic2, n, r, rm, src);

figure(1)

subplot(2,2,1)

plot(h)

title('麦克风阵列采集到语音信号')

axis([0,1500,0,0.4])

HT=h(1:1400);

[y,fs,nbits]=wavread('D:\毕设资料\谢安琪');

%sn=y(1:5000);

sn=y;

N=length(HT);

L=length(sn);

s1=zeros(1,L);

for n=2:N

for m=1:n-1

s1(n)=s1(n)+HT(m)*sn(n-m+1);

end

end

for n=N+1:L

for m=1:N

s1(n)=s1(n)+HT(m)*sn(n-m);

end

end

subplot(2,2,2)

plot(s1)

xlabel('第二个麦克风')

axis([0,100000,-0.6,0.6])

%%%%%%%%  s1为第二个麦克风的信号 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

n1=8;

h1=rir(fs, mic1, n1, r, rm, src);

%figure(3)

%subplot(2,1,1)

%plot(h1)

HT1=h1(1:2500);

N=length(HT1);

L=length(sn);

s2=zeros(1,L);

for n=2:N

for m=1:n-1

s2(n)=s2(n)+HT1(m)*sn(n-m+1);

end

end

for n=N+1:L

for m=1:N

s2(n)=s2(n)+HT1(m)*sn(n-m);

end

end

subplot(2,2,3)

plot(s2)

xlabel('第一个麦克风')

axis([0,100000,-0.6,0.6])

%%%%%%%%%%%% s2为第一个麦克风的信号  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

n2=8;

h2=rir(fs, mic3, n2, r, rm, src);

%figure(4)

% subplot(2,1,1)

% plot(h2)

HT2=h2(1:1800);

N=length(HT2);

L=length(sn);

s3=zeros(1,L);

for n=2:N

for m=1:n-1

s3(n)=s3(n)+HT2(m)*sn(n-m);

end

end

for n=N+1:L

for m=1:N

s3(n)=s3(n)+HT2(m)*sn(n-m);

end

end

subplot(2,2,4)

plot(s3)

xlabel('第三个麦克风')

axis([0,100000,-0.6,0.6])

%%%%%%%%%%%%%%%%%%%  s3为第三个麦克风的信号 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%分帧(200ms(2205个样点)，重叠50ms(约550个样点),加窗(hanning)

pp=hanning(1,2205);

LK=2205;LM=550;

LP=9;%LK+(LK-1)*(LF-2)-(LM)*(LF-1)+(LK-1)=L=16000；

cc1=zeros(9,LK*2-1); %用于存放互相关系数c1

cc2=zeros(9,LK*2-1); %用于存放互相关系数c2

phil1=zeros(9,LK);%用于存放第二个麦克风的希尔伯特包络；

phil2=zeros(9,LK);%用于存放第一个麦克风希尔伯特包络；

phil3=zeros(9,LK);%用于存放第三个麦克风希尔伯特包络；

for LF=1:LP

sh1(1:LK)=s1(LK+(LK-1)*(LF-2)-(LM)*(LF-1): LK+(LK-1)*(LF-2)-(LM)*(LF-1)+(LK-1)); %取第二个麦克风的信号帧

sh2(1:LK)=s2(LK+(LK-1)*(LF-2)-(LM)*(LF-1): LK+(LK-1)*(LF-2)-(LM)*(LF-1)+(LK-1)); %取第一个麦克风的信号帧

sh3(1:LK)=s3(LK+(LK-1)*(LF-2)-(LM)*(LF-1): LK+(LK-1)*(LF-2)-(LM)*(LF-1)+(LK-1)); %取第三个麦克风的信号帧

%%%%%%%对第二个麦克风的信号帧进行处理求希尔伯特包络  %%%%%%%%%%%%%%%%%%%%

M=length(sh1);

sp=zeros(1,M);

err=zeros(1,M);

Nt=10;

[A,E] = lpc(sh1,Nt);

for n=2:Nt

for t=2:n

sp(n)=sp(n)-A(t)*sh1(n-t+1);

end

err(n)=sh1(n)-sp(n);

end

for n=Nt+1:M

for t=2:Nt+1

sp(n)=sp(n)-A(t)*sh1(n-t+1);

end

err(n)=sh1(n)-sp(n);

end

%希尔伯特包络

errh=hilbert(err);

for n=1:M

erh(n)=imag(errh(n))^2;

er(n)=err(n)^2;

error(n)=sqrt((er(n)+erh(n)));

end

phil1(LF,:)=error;

%%%%%%%对第一个麦克风的信号帧进行处理求希尔伯特包络  %%%%%%%%%%%%%%%%%%%%

M=length(sh2);

spt=zeros(1,M);

errt=zeros(1,M);

Nt=10;

[At,Et] = lpc(sh2,Nt) ;

for n=2:Nt

for t=2:n

spt(n)=spt(n)-At(t)*sh2(n-t+1);

end

errt(n)=sh2(n)-spt(n);

end

for n=Nt+1:M

for t=2:Nt+1

spt(n)=spt(n)-At(t)*sh2(n-t+1);

end

errt(n)=sh2(n)-spt(n);

end

%希尔伯特包络

errht=hilbert(errt);

for n=1:M

erht(n)=imag(errht(n))^2;

ert(n)=errt(n)^2;

errort(n)=sqrt((ert(n)+erht(n)));

end

phil2(LF,:)=errort;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 求两个希尔伯特包络的互相关

c1 = xcorr(error,errort);

for n=1:length(c1)

if c1(n)==max(c1)

tg12(LF)=abs(n-length(c1)/2)/fs;

end

end

cc1(LF,:)=c1;

figure(4)

plot(tg12,'c+');

hold

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%对第三个麦克风的信号帧进行处理求希尔伯特包络  %%%%%%%%%%%%%%%%%%%%

M=length(sh3);

spt2=zeros(1,M);

errt2=zeros(1,M);

Nt=10;

[At2,Et2] = lpc(sh3,Nt) ;

for n=2:Nt

for t=2:n

spt2(n)=spt2(n)-At2(t)*sh3(n-t+1);

end

errt2(n)=sh3(n)-spt2(n);

end

for n=Nt+1:M

for t=2:Nt+1

spt2(n)=spt2(n)-At2(t)*sh3(n-t+1);

end

errt2(n)=sh3(n)-spt2(n);

end

%希尔伯特包络

errht2=hilbert(errt2);

for n=1:M

erht2(n)=imag(errht2(n))^2;

ert2(n)=errt2(n)^2;

errort2(n)=sqrt((ert2(n)+erht2(n)));

end

phil3(LF,:)=errort2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 求两个希尔伯特包络的互相关

c2 = xcorr(error,errort2);

for n=1:length(c2)

if c2(n)==max(c2)

tg32(LF)=abs(n-length(c2)/2)/fs;

end

end

cc2(LF,:)=c2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5

%%%%%%%%%%%%%%%%进行声源定位%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tg=tg12(LF)

tp=tg32(LF) %时间延迟

sita(LF)=asin((c*(tg+tp))/(2*d));

rl(LF)=((2*(d^2)-(c^2)*((tg)^2+(tp)^2)))/(2*c*(tg-tp));

tger(LF)=(tg-t12)/t12;

tper(LF)=(tp-t32)/t32;

figure(5)

subplot(2,1,1)

plot(tger,'*');

subplot(2,1,2)

plot(tper,'+');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

end

figure(4)

subplot(3,1,1)

plot(tg12,'c+');

hold on

plot(tt12,'r*')

hold on

subplot(3,1,2)

plot(tg32,'o');

hold on

plot(tt32,'y-');

hold on

subplot(3,1,3)

hold on

plot(tg12,'+');

plot(tt12,'r*')

plot(tt32,'rp');

plot(tg32,'o');

hold on

figure(5)

subplot(2,1,1)

plot(tger,'*');

subplot(2,1,2)

plot(tper,'+');

hold on