7站声源定位.zip_7站 声源 定位_strongeridc_声源_声源定位_定位

%function X = TaylorAlgorithm(BSN, MSP, Radius, Noise) function X = TaylorAlgorithm(BSN, MSP, Radius) % %TAYLORALGORITHM 本函数用于声源定位中TDOA的TAYLOR算法 % - BSN 为基站个数，3 < BSN <= 7； % - MS 声源标称位置 % - Noise 测距误差方差． % - R 为测量半径； % - X 为声源经算法处理后的位置. %See also: TaylorAlgorithm.m % 参数检查： %if nargout ~= 1 & nargout~=0, % error('Too many output arguments.'); %end %if nargin ~= 4, % error('Wrong number of input arguments.'); %end % 初始参数： BS = NetworkTop(BSN); MS = Radius*MSP; % TDOA协方差矩阵Q： Q = eye(BSN-1); c=340; % 初始估计位置： iEP = MS; % R0: for i = 1: BSN, MeaDist(i) = sqrt((MS(1) - BS(1,i))^2 + (MS(2) - BS(2,i))^2+(MS(3) - BS(3,i))^2); end for i = 1: BSN-1, R0(i) = MeaDist(i+1) - MeaDist(1); %+ Noise*rand(1); %TDOA测量值 end % 算法开始： %延时计算 [sound1,Fs] = wavread('151.wav'); [sound2,Fs] = wavread('152.wav'); [sound3,Fs] = wavread('153.wav'); %读取wav文件 s1=sound1(:,1)'; s2=sound2(:,1)'; s3=sound3(:,1)'; figure; subplot(3,1,1),plot(s1) subplot(3,1,2),plot(s2) subplot(3,1,3),plot(s3) %subplot(s1,s2,s3); %figure; %plot(s2); %figure; %plot(s3); figure; y1=xcorr(s2,s1); %互相关计算时延 y2=xcorr(s3,s1); %互相关计算时延 subplot(2,1,1),plot(abs(y1)) subplot(2,1,2),plot(abs(y2)) %figure; %plot(abs(y1)); %figure; %plot(abs(y2)); Yt(1)=(find(y1==max(y1))-length(s1))/Fs; Yt(2)=(find(y2==max(y2))-length(s1))/Fs; %延迟 %for n = 1:60,%最小二乘法 % Rn: R1 = sqrt(iEP(1)^2 + iEP(2)^2+iEP(3)^2); for i =1: BSN-1, R(i) = sqrt((iEP(1) - BS(1,i))^2 + (iEP(2) - BS(2,i))^2+(iEP(3) - BS(3,i))^2); end %for n = 1:2, % Rn: R1 = sqrt(iEP(1)^2 + iEP(2)^2+iEP(3)^2); for i =1: BSN-1, R(i) = sqrt((iEP(1) - BS(1,i+1))^2 + (iEP(2) - BS(2,i+1))^2+(iEP(3) - BS(3,i+1))^2); end % ht: for i = 1: BSN-1, %h(i) = R0(i)/c - (R(i) - R1)/c;%测距误差 h(i)=c*Yt(i)/-10; end ht = h'; % Gt: for i = 1: BSN-1, Gt(i, 1) = -iEP(1)/R1 - (BS(1, i+1) - iEP(1))/R(i); Gt(i, 2) = -iEP(2)/R1 - (BS(2, i+1) - iEP(2))/R(i); Gt(i, 3) = -iEP(3)/R1 - (BS(3, i+1) - iEP(3))/R(i); end % delt: delt = (inv(Gt'*inv(Q)*Gt)*Gt'*inv(Q)*ht)/10; EP = iEP + delt'; iEP = EP; %end % 结果输出: if nargout == 1, X = EP; elseif nargout == 0, disp(EP); end