TDOA声源定位

clear all; clc; close all;
d=0.5;
x1=0;y1=0;z1=0;%传声器位置   后面的传声器位置也要改
x2=0;y2=0;z2=1.125;
x3=0;y3=1.2;z3=0;
x4=1.2;y4=0;z4=0;
x5=-1.2;y5=0;z5=0;
x6=0;y6=0.6;z6=0;

x0=2;y0=6;z0=3;%声源位置设置处
x00=10;y00=20;z00=10;

r10=（（x1-x0）^2+（y1-y0）^2+（z1-z0）^2）^（1/2）;%声源（x0.y0.z0）到传声器的距离
r20=（（x2-x0）^2+（y2-y0）^2+（z2-z0）^2）^（1/2）;
r30=（（x3-x0）^2+（y3-y0）^2+（z3-z0）^2）^（1/2）;
r40=（（x4-x0）^2+（y4-y0）^2+（z4-z0）^2）^（1/2）;
r50=（（x5-x0）^2+（y5-y0）^2+（z5-z0）^2）^（1/2）;
r60=（（x6-x0）^2+（y6-y0）^2+（z6-z0）^2）^（1/2）;

r100=（（x1-x00）^2+（y1-y00）^2+（z1-z00）^2）^（1/2）;%声源（x00.y00.z00）到传声器的距离
r200=（（x2-x00）^2+（y2-y00）^2+（z2-z00）^2）^（1/2）;
r300=（（x3-x00）^2+（y3-y00）^2+（z3-z00）^2）^（1/2）;
r400=（（x4-x00）^2+（y4-y00）^2+（z4-z00）^2）^（1/2）;
r500=（（x5-x00）^2+（y5-y00）^2+（z5-z00）^2）^（1/2）;
r600=（（x6-x00）^2+（y6-y00）^2+（z6-z00）^2）^（1/2）;

f1=340;f2=360;p1=50;p2=77;c=340;
f3=350;p3=100;f4=500;p4=150;
f5=480;p5=80;f6=520;p6=82;
f7=100;p7=30; %噪声频段

N=100000;%长度
Fs=200000;%采样频率
n=0:N-1;
t=n/Fs;   %时间序列