def optimizeIntersectionPoint_nonLinear_numeric(linGoal, curveA, curveB):
# Initial Guess for t_A and t_B
# hier am Anfang ohne max_iterations und großes startIntervall, damit Qualität der Ausgangswerte hoch
startIntervall = getPoint(0, 1000)
endIntervall = intervallSearch_t_closest(intervall=startIntervall,
curve=curveA,
point=linGoal,
iteration=0,
max_iterations=100)
tA = np.average(endIntervall)
endIntervall = intervallSearch_t_closest(intervall=startIntervall,
curve=curveB,
point=linGoal,
iteration=0,
max_iterations=100)
tB = np.average(endIntervall)
# Determine indices
p1, p2 = getPoint_on_tCurve(curveA, tA)
p, iA = getClosestPoint(linGoal, [p1, p2])
p1, p2 = getPoint_on_tCurve(curveB, tB)
p, iB = getClosestPoint(linGoal, [p1, p2])
# Optimize t_A and t_B iteratively
# Parameters
maxIt = 50
startStepSize = 1.0
intervallSearchMaxIt = 50
epsilon = 0.00001
# working variables
it = 0
stepSize = startStepSize
pointA = getPoint_on_tCurve_idx(curveA, tA, iA)
pointB = getPoint_on_tCurve_idx(curveB, tB, iB)
oldDist = distance(pointA, pointB)
while (oldDist > epsilon):
tA, tB = gridSearch(tA, tB, curveA, curveB, iA, iB, stepSize=stepSize)
endIntervall = intervallSearch_t_closest(
intervall=getPoint(tB - stepSize, tB + stepSize),
curve=curveB,
point=getPoint_on_tCurve_idx(curveA, tA, iA),
iteration=0,
max_iterations=intervallSearchMaxIt)
tB = np.average(endIntervall)
pointA = getPoint_on_tCurve_idx(curveA, tA, iA)
pointB = getPoint_on_tCurve_idx(curveB, tB, iB)
newDist = distance(pointA, pointB)
if (oldDist == newDist):
stepSize = stepSize / 2
oldDist = newDist
it += 1
if (it > maxIt):
break
return (pointA + pointB) / 2
def calcKs(signals, meta_data, source_pos, plot_):
#also plot them
if plot_:
plt.figure()
x = range(0, len(signals[0]))
for i in range(0, len(signals)):
plt.subplot(8, 1, i + 1)
plt.plot(x, signals[i])
plt.show()
#main calculation
array = list()
array.append(getPoint(-0.45-0.04,+0.90))
array.append(getPoint(+0.45+0.04,+0.90))
array.append(getPoint(0,+0.45))
array.append(getPoint(-0.45-0.04,0))
array.append(getPoint(+0.45+0.04,0))
array.append(getPoint(0,-0.45))
array.append(getPoint(-0.45-0.04,-0.90))
array.append(getPoint(+0.45+0.04,-0.90))
distances = [] #TODO calculate them
for i in range(0, len(signals)):
distances.append(GeometryLibrary.distance(source_pos, array[i]))
speed_of_sound = 343.3
sample_rate = meta_data["sampling_rate"]
calib = CalibrationLibrary.Calibrator(signals, distances, speed_of_sound, sample_rate)
ks = calib.run_calibration()
return ks
def getMicrophonePositions_SIM_A(radius): # on circle
points = list()
points.append(
getPoint(radius * np.cos(4 * np.pi / 5),
radius * np.sin(4 * np.pi / 5)))
points.append(
getPoint(radius * np.cos(3 * np.pi / 5),
radius * np.sin(3 * np.pi / 5)))
points.append(
getPoint(radius * np.cos(2 * np.pi / 5),
radius * np.sin(2 * np.pi / 5)))
points.append(
getPoint(radius * np.cos(1 * np.pi / 5),
radius * np.sin(1 * np.pi / 5)))
return points
def intervallSearch_t_closest(intervall, curve, point, iteration,
max_iterations):
pointA1, pointA2 = getPoint_on_tCurve(curve, intervall[0])
pointC1, pointC2 = getPoint_on_tCurve(curve, intervall[1])
listA = list()
listA.append(pointA1)
listA.append(pointA2)
listC = list()
listC.append(pointC1)
listC.append(pointC2)
pointA, ia = getClosestPoint(point, listA)
pointC, ic = getClosestPoint(point, listC)
valueA = np.linalg.norm(point - pointA)
valueC = np.linalg.norm(point - pointC)
if (valueA < valueC): # it is in the Intervall A to B
intervall = getPoint(intervall[0], np.average(intervall))
else:
intervall = getPoint(np.average(intervall), intervall[1])
if (iteration < max_iterations):
return intervallSearch_t_closest(intervall, curve, point,
iteration + 1, max_iterations)
else:
return intervall
def plausibleFilter_TDOA(solutions):
result = list()
for p in solutions:
if(p[1]>0):
result.append(p)
if(len(result)>1):
if(np.linalg.norm(result[0])>np.linalg.norm(result[1])):
return result[0]
else:
return result[1]
elif(len(result)==1):
return result[0]
else:
return getPoint(0,0)
def getRealDeltaS(source_pos, micAPos, micBPos):
return distance(micAPos, source_pos) - distance(micBPos, source_pos)
def getRealTDOA(source_pos, micAPos, micBPos):
return getRealDeltaS(source_pos, micAPos, micBPos) / 343.3
# Load TimeFrames for Measurements
keyList, limitList = loadTimeWindows()
# Define Array 5
micList = list()
num_mic = 8
mic_dist = 0.38
for i in range(0,num_mic):
micList.append(getPoint(mic_dist*np.sin(angle_radians(360/8*(i))),mic_dist*np.cos(angle_radians(360/8*(i)))))
# Load Measurements
signals = ["CH", "SX"]
distances = ["10", "20", "40", "60", "80"]
anglesA4 = ["00", "22,5", "45", "67,5", "90"]
anglesA5 = ["00", "10", "25"]
file = "results_SSL_LIN_A5M_DesignC.txt"
f = open(file, 'w')
print("arr ; signal ; angle ; dist ; pNoise ; pSigna ; snr_DB ; TDOA_real1 ; TDOA_real2 ; TDOA_CSOM1 ; TDOA_CSOM2 ; angleReal ; angleLIN ; angleNOL ; distanceReal ; distanceLIN ; distanceNOL")
print("arr ; signal ; angle ; dist ; pNoise ; pSigna ; snr_DB ; TDOA_real1 ; TDOA_real2 ; TDOA_CSOM1 ; TDOA_CSOM2 ; angleReal ; angleLIN ; angleNOL ; distanceReal ; distanceLIN ; distanceNOL", file=f)
arr = "A5"
for signal in signals:
for dist in distances:
angles = [75, 90] #[0, 15, 45, 75, 90]
f = open('out-dist-5,00mA.txt', 'w')
print("dis;ang;source_pos;SNR;TDOA_real;TDOA_CSOM;TDOA_GCCP")
print("dis;ang;source_pos;SNR;TDOA_real;TDOA_CSOM;TDOA_GCCP", file=f)
mic_dist = 5
for dis in distances:
for ang in angles:
SNRls = list()
TDOA_realls = list()
TDOA_csomls = list()
for n in range(0, N):
# Adjust Configurations
micA = getPoint(-mic_dist / 2, 0)
micB = getPoint(mic_dist / 2, 0)
noise_microphone = 0.003
noise_environment = 0.04
noise_source = 0.01
source_pos = getPoint(dis * np.sin(angle_radians(ang)),
dis * np.cos(angle_radians(ang)))
config = updateConfig(config, micA, micB, noise_microphone,
noise_environment, noise_source, source_pos)
# Starte Simulation
loaded = simulate(config, config["source_position"],
signal_function)
signals = loaded.get_measurements()
meta = loaded.get_meta_data()
signalA = signals[0]
for xval in [0.6, 0.65, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95]:
l = list()
for dis in distances:
# print(dis)
lol = list()
for n in range(0, 10):
# dis = 80
ang = 0
# Adjust Configurations
micList = list()
for i in range(0, num_mic):
micList.append(
getPoint(
mic_dist * np.sin(angle_radians(360 / 8 * (2 - i))),
mic_dist * np.cos(angle_radians(360 / 8 * (2 - i)))))
noise_microphone = 0.003
noise_environment = 0.04
noise_source = 0.01
source_pos = getPoint(dis * np.sin(angle_radians(ang)),
dis * np.cos(angle_radians(ang)))
config = updateConfig(config, micList, noise_microphone,
noise_environment, noise_source, source_pos)
# Signal Simulation
loaded = simulate(config, config["source_position"],
signal_function)
signals = loaded.get_measurements()
meta = loaded.get_meta_data()
signalsFiltered = list()
distances = [1, 5, 10, 15, 20, 25, 30, 35, 40]
angles = [0, 10, 20, 30, 45, 60, 70, 80, 90]
f = open('results-0.80.txt', 'w')
print("dis ; ang ; K_true ; K_estim ; dist_estim")
print("dis ; ang ; K_true ; K_estim ; dist_estim", file=f)
for ang in angles:
for dis in distances:
for n in range(0, N):
# Adjust Configurations
micList = list()
for i in range(0, num_mic):
micList.append(
getPoint(
mic_dist * np.sin(angle_radians(360 / 8 * (2 - i))),
mic_dist * np.cos(angle_radians(360 / 8 * (2 - i)))))
noise_microphone = 0.003
noise_environment = 0.04
noise_source = 0.01
source_pos = getPoint(dis * np.sin(angle_radians(ang)),
dis * np.cos(angle_radians(ang)))
config = updateConfig(config, micList, noise_microphone,
noise_environment, noise_source, source_pos)
# Signal Simulation
loaded = simulate(config, config["source_position"],
signal_function)
signals = loaded.get_measurements()
meta = loaded.get_meta_data()
signalsFiltered = list()
curveB = get_tCurve(micList[indC], micList[indD], s_tdoaMAT[indD][indC])
estimationNOL = optimizeIntersectionPoint_nonLinear_numeric(
estimationLIN, curveA, curveB)
return estimationNOL
def getRealDeltaS(source_pos, micAPos, micBPos):
return distance(micAPos, source_pos) - distance(micBPos, source_pos)
# Lade Konfiguration
config = load_configs("configEXP.json")[0]
# Setze Mikrofon Positionen
micList = calculateMicrophoneArray_2(array_radius=0.5,
array_center=getPoint(0, 0))
config["microphone_noise_mus"] = list()
config["microphone_noise_sigmas"] = list()
config["microphone_noise_amplitudes"] = list()
config["microphone_positions"] = list()
for m in micList:
config["microphone_noise_mus"].append(0)
config["microphone_noise_sigmas"].append(0) #0.003)
config["microphone_noise_amplitudes"].append(1)
config["microphone_positions"].append(m)
# Setze Tonquelle
config["source_position"] = [10, 10]
source_pos = convertPoint(config["source_position"])
# Starte Simulation
def convertPoint(p):
return getPoint(p[0], p[1])
steep1 = KarstenDOA_calculateSteep_linear_simple(distance(micA, micB),
std1)
steep2 = KarstenDOA_calculateSteep_linear_simple(distance(micC, micD),
std2)
solutions, m1_a, m1_b, m2_a, m2_b, b1_a, b1_b, b2_a, b2_b = getMicrophonePair_DOA_Intersection_linear(
micA, micB, micC, micD, steep1, steep2)
estimation = plausibleFilter_TDOA(solutions)
return estimation
array = "D"
mic_dist = 0.4
# Array errechnen
if (array == "A"):
micA = getPoint(-mic_dist * 3 / 2, 0)
micB = getPoint(-mic_dist * 1 / 2, 0)
micC = getPoint(+mic_dist * 1 / 2, 0)
micD = getPoint(+mic_dist * 3 / 2, 0)
if (array == "B"):
micA = getPoint(-mic_dist * 1 / 2, +mic_dist * 1 / 2)
micB = getPoint(+mic_dist * 1 / 2, +mic_dist * 1 / 2)
micC = getPoint(-mic_dist * 1 / 2, -mic_dist * 1 / 2)
micD = getPoint(+mic_dist * 1 / 2, -mic_dist * 1 / 2)
if (array == "C"):
micA = getPoint(-mic_dist * 3 / 2, 0)
micB = getPoint(-mic_dist * 1 / 2, 0)
micC = getPoint(+mic_dist * 1 / 2, 0)
micD = getPoint(+mic_dist * 3 / 2, 0)
if (array == "D"):
micA = getPoint(-mic_dist * 1 / 2, +mic_dist * 1 / 2)
xval = 0.7
distances = [1,5,10,20,40,60,80,100]
angles = [0, 10, 20, 30, 45, 60, 70, 80, 90]
f = open('results-0.70.txt', 'w')
print("dis ; ang ; K_true ; K_estim ; dist_estim")
print("dis ; ang ; K_true ; K_estim ; dist_estim", file=f)
for ang in angles:
for dis in distances:
for n in range(0,N):
# Adjust Configurations
micList = list()
for i in range(0,num_mic):
micList.append(getPoint(mic_dist*np.sin(angle_radians(360/8*(2-i))),mic_dist*np.cos(angle_radians(360/8*(2-i)))))
noise_microphone = 0.003
noise_environment = 0.04
noise_source = 0.01
source_pos = getPoint(dis*np.sin(angle_radians(ang)),dis*np.cos(angle_radians(ang)))
config = updateConfig(config, micList, noise_microphone, noise_environment, noise_source, source_pos)
# Signal Simulation
loaded = simulate(config, config["source_position"], signal_function)
signals = loaded.get_measurements()
meta = loaded.get_meta_data()
signalsFiltered = list()
signalsPower = list()
signalsSNR = list()
for s in signals:
sf = butterWorthFilter(s, meta["sampling_rate"], 2000)
def getMicrophonePositions_SIM_C(radius): # in straight line
points = list()
points.append(getPoint(-radius, 0))
points.append(getPoint(0, 0))
points.append(getPoint(+radius, 0))
return points
def getMicrophonePositions_SIM_B(radius): # in triangle
points = list()
points.append(getPoint(-radius, 0))
points.append(getPoint(0, +radius))
points.append(getPoint(+radius, 0))
return points
error1 = list()
error2 = list()
for d in data:
error1.append(float(d[7]))
error2.append(float(d[11]))
errmax1 = np.max(error1)
errmax2 = np.max(error2)
plt.subplot(1, 2, 1)
plt.grid()
for d in data:
dist = float(d[0])
angl = float(d[1])
distError = float(d[7])
relError = distError / dist
point = getPoint(dist * np.sin(angle_radians(angl)),
dist * np.cos(angle_radians(angl)))
plt.scatter(point[0], point[1], s=1000 * distError / errmax1, c="Black")
#plt.colorbar()
plt.subplot(1, 2, 2)
plt.grid()
for d in data:
dist = float(d[0])
angl = float(d[1])
anglError = float(d[11])
point = getPoint(dist * np.sin(angle_radians(angl)),
dist * np.cos(angle_radians(angl)))
plt.scatter(point[0], point[1], s=1000 * anglError / errmax2, c="Black")
#plt.colorbar()
for dis in distances:
for ang in angles:
TDOA_realls1 = list()
TDOA_realls2 = list()
TDOA_csomls1 = list()
TDOA_csomls2 = list()
TDOAstdErr1 = list()
TDOAstdErr2 = list()
ANGLE_stdErrLIN = list()
DISTA_stdErrLIN = list()
ANGLE_stdErrNOL = list()
DISTA_stdErrNOL = list()
for n in range(0, N):
# Adjust Configurations
micA = getPoint(-mic_dist * 1 / 2, +mic_dist * 1 / 2)
micB = getPoint(-mic_dist * 1 / 2, -mic_dist * 1 / 2)
micC = getPoint(+mic_dist * 1 / 2, 0)
micD = getPoint(+mic_dist * 3 / 2, 0)
noise_microphone = 0.003
noise_environment = 0.04
noise_source = 0.01
source_pos = getPoint(dis * np.sin(angle_radians(ang)),
dis * np.cos(angle_radians(ang)))
config = updateConfig(config, micA, micB, micC, micD,
noise_microphone, noise_environment,
noise_source, source_pos)
## Starte Simulation
loaded = simulate(config, config["source_position"],
def getRealTDOA(source_pos, micAPos, micBPos):
return getRealDeltaS(source_pos, micAPos, micBPos) / 343.3
# Load TimeFrames for Measurements
keyList, limitList = loadTimeWindows()
# Define Array 5
micList = list()
num_mic = 8
mic_dist = 0.38
for i in range(0, num_mic):
micList.append(
getPoint(mic_dist * np.sin(angle_radians(360 / 8 * (i))),
mic_dist * np.cos(angle_radians(360 / 8 * (i)))))
# Load Measurements
signals = ["CH", "SX"]
distances = ["10", "20", "40", "60", "80"]
anglesA4 = ["00", "22,5", "45", "67,5", "90"]
anglesA5 = ["00", "10", "25"]
file = "results_SSL_LIN_A5M_DesignC.txt"
f = open(file, 'w')
print(
"arr ; signal ; angle ; dist ; pNoise ; pSigna ; snr_DB ; TDOA_real1 ; TDOA_real2 ; TDOA_CSOM1 ; TDOA_CSOM2 ; angleReal ; angleLIN ; angleNOL ; distanceReal ; distanceLIN ; distanceNOL"
)
print(
"arr ; signal ; angle ; dist ; pNoise ; pSigna ; snr_DB ; TDOA_real1 ; TDOA_real2 ; TDOA_CSOM1 ; TDOA_CSOM2 ; angleReal ; angleLIN ; angleNOL ; distanceReal ; distanceLIN ; distanceNOL",
file=f)
anglesA4 = ["00", "22,5", "45", "67,5", "90"]
anglesA5 = ["00", "10", "25"]
f = open("CalibrationResults.txt", 'w')
print("arr ; signal ; angle ; dist ; K1 ; K2 ; K3 ; K4 ; K5 ; K6 ; K7 ; K8")
print("arr ; signal ; angle ; dist ; K1 ; K2 ; K3 ; K4 ; K5 ; K6 ; K7 ; K8",
file=f)
arr = "A5"
for signal in signals:
for dist in distances:
for angle in anglesA5:
# Determine Sound Source Pos
source_pos = getPoint(
float(dist) *
np.sin(angle_radians(float(angle.replace(",", ".")))),
float(dist) *
np.cos(angle_radians(float(angle.replace(",", ".")))))
# Load Data
data, meta_data = load_measurement(arr, signal, angle, dist)
# Determine SNR
index = keyList.index(arr + "," + signal + "," + dist + "," +
angle)
time_windowNoise = {
"from": limitList[index][0],
"to": limitList[index][1]
}
time_windowSignal = {
"from": limitList[index][2],
rA = estim_K/np.sqrt(powerA)
rB = estim_K/np.sqrt(powerB)
estimPoints = list()
for i in range(0,7):
for j in range(0,7):
if(i!=j):
rI = estim_K/np.sqrt(getSignalPower_UsingTime_AverageFree(np.asarray(signals[i])))
rJ = estim_K/np.sqrt(getSignalPower_UsingTime_AverageFree(np.asarray(signals[j])))
solutions = getIntersectionPointsCircle(convertPoint(config["microphone_positions"][i]), rI, convertPoint(config["microphone_positions"][j]), rJ)
if(len(solutions)!=0):
estimPoint = plausibleFilter_AMP(solutions)
estimPoints.append(estimPoint)
# Ergebnisse auswerten
array_center = getPoint(0,0)
distList = list()
anglList = list()
for p in estimPoints:
distList.append(distance(array_center,convertPoint(p)))
anglList.append(getAngle_angle1(array_center, convertPoint(p)))
d = np.average(distList)
a = np.average(anglList)
estimPoint = getPoint(d*np.cos(a),d*np.sin(a))
# Plot Results
fig = plt.figure(figsize=(15,5))
plt.subplot(1,3,1)
plt.plot(K_list, label="sorted K list")
plt.plot(np.arange(len(K_list)-len(K_list_filt),len(K_list)),K_list_filt, label="filtered K list")
def getRealTDOA(source_pos, micAPos, micBPos):
return getRealDeltaS(source_pos, micAPos, micBPos) / 343.3
def getRealDeltaS(source_pos, micAPos, micBPos):
return distance(micAPos, source_pos) - distance(micBPos, source_pos)
# Load TimeFrames for Measurements
keyList, limitList = loadTimeWindows()
# Define Array 5
micList = list()
num_mic = 8
num_mic = 8
micList.append(getPoint(-0.45 - 0.04, +0.90))
micList.append(getPoint(+0.45 + 0.04, +0.90))
micList.append(getPoint(0, +0.45))
micList.append(getPoint(-0.45 - 0.04, 0))
micList.append(getPoint(+0.45 + 0.04, 0))
micList.append(getPoint(0, -0.45))
micList.append(getPoint(-0.45 - 0.04, -0.90))
micList.append(getPoint(+0.45 + 0.04, -0.90))
# Load Measurements
signals = ["CH", "SX"]
distances = ["10", "20", "40", "60", "80"]
anglesA4 = ["00", "22,5", "45", "67,5", "90"]
anglesA5 = ["00", "10", "25"]
file = "results_A4_SX.txt"
# Imports from GeometryLibrary import getPoint, distance from SslLibrary import SSL_TDOA_LN, SSL_TDOA_NL, SSL_AMPL from GraphicLibrary import drawPoint, drawPointL, initDrawing, finishDrawingL from MicrophonePositionLibrary import getMicrophonePositions_SIM_A, getMicrophonePositions_SIM_B, getMicrophonePositions_SIM_C, getMicrophonePositions4_TDOA, getMicrophonePositions4_AMP # Define Physics c_speed = 300 # Define Microphones micPosList = getMicrophonePositions_SIM_C(0.3) micPosList_TDOA = getMicrophonePositions4_TDOA(micPosList) micPosList_AMP = getMicrophonePositions4_AMP(micPosList) # Define Soundsource soundSource = getPoint(10, 5) # Signal Processing which is fake tdoa1 = (1/c_speed) * (distance(micPosList_TDOA[0], soundSource) - distance(micPosList_TDOA[1], soundSource)) tdoa2 = (1/c_speed) * (distance(micPosList_TDOA[2], soundSource) - distance(micPosList_TDOA[3], soundSource)) distA = distance(micPosList_AMP[0], soundSource) distB = distance(micPosList_AMP[1], soundSource) # Calculate Estimations point1 = SSL_TDOA_LN(micPosList_TDOA, tdoa1, tdoa2, c_speed) point2 = SSL_TDOA_NL(micPosList_TDOA, tdoa1, tdoa2, c_speed, point1) point3 = SSL_AMPL(micPosList_AMP, distA, distB) # View Results fig, ax = initDrawing()
