def SSL_TDOA_LIN(tdoa1, tdoa2, micA, micB, micC, micD):
std1 = tdoa1 * 343.2
std2 = tdoa2 * 343.2
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
def SSL_TDOA_LIN(s_tdoaMAT, micPosList, indA, indB, indC, indD):
std1 = s_tdoaMAT[indB][indA]
std2 = s_tdoaMAT[indD][indC]
rstd1 = getRealDeltaS(source_pos, micPosList[indA], micPosList[indB])
rstd2 = getRealDeltaS(source_pos, micPosList[indC], micPosList[indD])
print("STDOA 1: ", std1, rstd1)
print("STDOA 2: ", std2, rstd2)
steep1 = KarstenDOA_calculateSteep_linear_simple(
distance(micList[indA], micList[indB]), s_tdoaMAT[indB][indA])
steep2 = KarstenDOA_calculateSteep_linear_simple(
distance(micList[indC], micList[indD]), s_tdoaMAT[indD][indC])
solutions, m1_a, m1_b, m2_a, m2_b, b1_a, b1_b, b2_a, b2_b = getMicrophonePair_DOA_Intersection_linear(
micList[indA], micList[indB], micList[indC], micList[indD], steep1,
steep2)
estimation = plausibleFilter_TDOA(solutions)
return estimation
def getRealDeltaS(source_pos, micAPos, micBPos):
return distance(micAPos, source_pos) - distance(micBPos, source_pos)
2000)
TDOA_CSOM1, t, csom = tdoa_csom(signalAF,
signalBF,
fs=meta_data["sampling_rate"],
window=200)
TDOA_CSOM2, t, csom = tdoa_csom(signalCF,
signalDF,
fs=meta_data["sampling_rate"],
window=200)
TDOA_real1 = getRealTDOA(source_pos, micA, micB)
TDOA_real2 = getRealTDOA(source_pos, micC, micD)
# Get Real Data
angleReal = 90 - angle_degree(
getAngle_angle1(getPoint(0, 0), source_pos))
distanceReal = distance(source_pos, getPoint(0, 0))
# Calculate True K
K_true = 0
for k in range(0, len(micList)):
K_true += signalsPower[k] * distance(
micList[k], source_pos) * distance(micList[k], source_pos)
K_true /= len(micList)
# Calculate K estimation
K_estim_exakt = list()
K_estim_noise = list()
for i in range(0, len(micList)):
for j in range(0, len(micList)):
if (i != j):
a = getRealTDOA(source_pos, micList[i], micList[j])
signalDF,
fs=meta["sampling_rate"],
window=2000)
TDOA_real1 = getRealTDOA(source_pos, micA, micB)
TDOA_real2 = getRealTDOA(source_pos, micC, micD)
# SSL - TDOA_LIN
estimationLIN = SSL_TDOA_LIN(TDOA_CSOM1, TDOA_CSOM2, micA, micB,
micC, micD)
# SSL - TDOA_NOL
estimationNOL = SSL_TDOA_NOL(TDOA_CSOM1, TDOA_CSOM2, micA, micB,
micC, micD, estimationLIN)
# Distance
distanceReal = distance(source_pos, getPoint(0, 0))
angleReal = 90 - angle_degree(
getAngle_angle1(getPoint(0, 0), source_pos))
distanceLIN = distance(source_pos, estimationLIN)
distanceNOL = distance(source_pos, estimationNOL)
angleLIN = 90 - angle_degree(
getAngle_angle1(getPoint(0, 0), estimationLIN))
angleNOL = 90 - angle_degree(
getAngle_angle1(getPoint(0, 0), estimationNOL))
# print("angles ° ",angleReal,angleLIN,angleNOL)
# print("distances m ",distanceReal,distanceLIN, distanceNOL)
TDOA_error1 = np.sqrt(
(TDOA_real1 - TDOA_CSOM1) * (TDOA_real1 - TDOA_CSOM1))
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)
powerSig, powerNoi, snrFac, snrDB = getSNR(sf)
signalsFiltered.append(sf)
signalsPower.append(powerSig)
signalsSNR.append(snrDB)
# Calculate True K
K_true = 0
for k in range(0, len(micList)):
K_true += signalsPower[k] * distance(
micList[k], source_pos) * distance(micList[k], source_pos)
K_true /= len(micList)
# Calculate K estimation
K_estim_exakt = list()
K_estim_noise = list()
for i in range(0, len(micList)):
for j in range(0, len(micList)):
if (i != j):
a = getRealTDOA(source_pos, micList[i], micList[j])
b = getFakeTDOA(a, 48000)
K1_exakt, K2_exakt = estimateK_Pair(
signalsPower[i], signalsPower[j], micList[i],
micList[j], a * 343.2)
K1_noised, K2_noised = estimateK_Pair(
signalsPower[i], signalsPower[j], micList[i],
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() # Draw Microphones for p in micPosList:
# AMP Verfahren
FILTER_LOW = int(len(K_list)*0.4)
FILTER_HIGH = len(K_list)-1
K_list = [x for x in K_list if str(x) != 'nan']
K_list.sort()
K_list_filt = [x for x in K_list if x > 0.05] #K_list[FILTER_LOW-1:FILTER_HIGH]
estim_K = np.median(K_list_filt)
import matplotlib.pyplot as plt
#plt.plot(K_list)
#
real_K = 0
for i in range(0,7):
real_K += distance(convertPoint(config["microphone_positions"][i]), source_pos) * np.sqrt(getSignalPower_UsingTime_AverageFree(np.asarray(signals[i])))
real_K /= 8
print("Real K ",real_K)
print("Estim K ",estim_K)
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)
K1, K2 = estimateK_Pair(powerB, powerC, micB_pos, micC_pos, delta_s4)
K_list.append(K1)
K_list.append(K2)
K1, K2 = estimateK_Pair(powerB, powerD, micB_pos, micD_pos, delta_s5)
K_list.append(K1)
K_list.append(K2)
K1, K2 = estimateK_Pair(powerC, powerD, micC_pos, micD_pos, delta_s6)
K_list.append(K1)
K_list.append(K2)
K_list.sort()
import matplotlib.pyplot as plt
plt.plot(K_list)
real_K = distance(micA_pos, source_pos) * np.sqrt(powerA)
print("Real K ",real_K)
real_K = distance(micB_pos, source_pos) * np.sqrt(powerB)
print("Real K ",real_K)
real_K = distance(micC_pos, source_pos) * np.sqrt(powerC)
print("Real K ",real_K)
real_K = distance(micD_pos, source_pos) * np.sqrt(powerD)
print("Real K ",real_K)
print(" ")
print(" ")
print(distance(micA_pos, source_pos), "\t", real_K/np.sqrt(powerA))
print(distance(micB_pos, source_pos), "\t", real_K/np.sqrt(powerB))
print(distance(micC_pos, source_pos), "\t", real_K/np.sqrt(powerC))
print(distance(micD_pos, source_pos), "\t", real_K/np.sqrt(powerD))
K1, K2 = estimateK_Pair(powerA, powerB, micA_pos, micB_pos,
delta_s1)
K_list.append(K1)
K_list.append(K2)
# AMP Verfahren
K_list = [x for x in K_list if str(x) != 'nan']
K_list.sort()
K_list_filt = K_list[int(len(K_list) / 13 * 6):len(K_list) - 1]
estim_K = np.median(K_list_filt)
import matplotlib.pyplot as plt
#plt.plot(K_list)
#
real_K = distance(micA_pos, source_pos) * np.sqrt(powerA)
print("Real K ", real_K)
print("Estim K ", estim_K)
rA = estim_K / np.sqrt(powerA)
rB = estim_K / np.sqrt(powerB)
fig = plt.figure(figsize=(16, 8))
plt.subplot(1, 2, 1)
plt.plot(K_list, label="sorted K list")
plt.plot(np.arange(int(len(K_list) / 13 * 6),
len(K_list) - 1),
K_list_filt,
label="filtered K list")
powerD = getSignalPower_UsingTime_AverageFree(np.asarray(signals[3]))
powerSrc = getSignalPower_UsingTime_AverageFree(source_signal)
# Calculate TDOA
arr = array_parameters.ArrayParameters(config["microphone_positions"])
tdoa = basic_tdoa.BasicTDOA(loaded, 0, 0.0, arr)
tdoaMAT = tdoa.tdoa_gcc_phat(0.0)[1][0]
delta_n1 = tdoaMAT[1][0]
delta_n2 = tdoaMAT[3][2]
delta_t1 = delta_n1*meta["sampling_spacing"]
delta_t2 = delta_n2*meta["sampling_spacing"]
delta_s1 = delta_t1*343.2
delta_s2 = delta_t2*343.2
# TDOA LINEAR VERFAHREN
steep1 = KarstenDOA_calculateSteep_linear_simple(distance(micA_pos, micB_pos), delta_s1)
steep2 = KarstenDOA_calculateSteep_linear_simple(distance(micC_pos, micD_pos), delta_s2)
solutions, m1_a, m1_b, m2_a, m2_b, b1_a, b1_b, b2_a, b2_b = getMicrophonePair_DOA_Intersection_linear(micA_pos, micB_pos, micC_pos, micD_pos, steep1, steep2)
estimationLIN = plausibleFilter_TDOA(solutions)
curveA = get_tCurve(micA_pos, micB_pos, delta_s1)
curveB = get_tCurve(micC_pos, micD_pos, delta_s2)
estimationNOL = optimizeIntersectionPoint_nonLinear_numeric(estimationLIN, curveA, curveB)
# Kurven zur Darstellung
X_NOL_CURVE1, Y_NOL_CURVE1 = KarstenDOA_calculateCurve_nonlinear(micA_pos, micB_pos, delta_s1, res=0.01, rang=10)
X_LIN_CURVE1, Y_LIN_CURVE1 = KarstenDOA_calculateCurve_linear(micA_pos, micB_pos, delta_s1, res=0.01, rang=10)
X_NOL_CURVE2, Y_NOL_CURVE2 = KarstenDOA_calculateCurve_nonlinear(micC_pos, micD_pos, delta_s2, res=0.01, rang=10)
X_LIN_CURVE2, Y_LIN_CURVE2 = KarstenDOA_calculateCurve_linear(micC_pos, micD_pos, delta_s2, res=0.01, rang=10)
# Plot Data
z = np.asarray(s)
power = getSignalPower_UsingTime_AverageFree(np.asarray(s))
powers.append(power)
# print("microphone ",power,"W")
#print("Source Power ",getSignalPower_UsingTime_AverageFree(source_signal),"W")
# Calculate TDOA
arr = array_parameters.ArrayParameters(config["microphone_positions"])
tdoa = basic_tdoa.BasicTDOA(loaded, 0, 0.0, arr)
# Do Amplitude Verfahren
powerA = powers[0]
powerB = powers[1]
micA_pos = convertPoint(config["microphone_positions"][0])
micB_pos = convertPoint(config["microphone_positions"][1])
dTDOA = tdoa.tdoa_gcc_phat(0.0)[1][0][1][0]
dSDOA = dTDOA * meta["sampling_spacing"] * 343.2
K1, K2 = estimateK_Pair(powerA, powerB, micA_pos, micB_pos, dSDOA)
rA_1 = K1 / np.sqrt(powerA)
rA_2 = K2 / np.sqrt(powerA)
rB_1 = K1 / np.sqrt(powerB)
rB_2 = K2 / np.sqrt(powerB)
solutions = getIntersectionPointsCircle(micA_pos, rA_2, micB_pos, rB_2)
estimPos = getPlausibleOne(solutions[0], solutions[1])
sourcePos = convertPoint(config["source_position"])
print(i, config["source_position"], estimPos, " ", sourcePos, " > ",
distance(estimPos, sourcePos))
tdoa = basic_tdoa.BasicTDOA(loaded, 0, 0, arr)
tdoaMAT = tdoa.tdoa_gcc_phat(0.0)[1][0]
delta_n1 = tdoaMAT[1][0]
delta_n2 = tdoaMAT[3][2]
delta_t1 = delta_n1*meta["sampling_spacing"]
delta_t2 = delta_n2*meta["sampling_spacing"]
delta_s1 = delta_t1*343.2
delta_s2 = delta_t2*343.2
#
delta_tX1 = gcc_phat(np.asarray(signals[0]), np.asarray(signals[1]), fs=meta["sampling_rate"])[0]
delta_tX2 = gcc_phat(np.asarray(signals[2]), np.asarray(signals[3]), fs=meta["sampling_rate"])[0]
delta_sX1 = delta_tX1*343.2
delta_sX2 = delta_tX2*343.2
true_s1 = distance(source_pos, micA_pos) - distance(source_pos, micB_pos)
true_s2 = distance(source_pos, micC_pos) - distance(source_pos, micD_pos)
print(true_s1, delta_s1, true_s2, delta_s2, delta_sX1, delta_sX2)
# print(true_s1, true_s2, delta_sX1, delta_sX2)
# delta_s1 = delta_sX1
# delta_s2 = delta_sX2
# TDOA LINEAR VERFAHREN
steep1 = KarstenDOA_calculateSteep_linear_simple(distance(micA_pos, micB_pos), delta_s1)
steep2 = KarstenDOA_calculateSteep_linear_simple(distance(micC_pos, micD_pos), delta_s2)
solutions, m1_a, m1_b, m2_a, m2_b, b1_a, b1_b, b2_a, b2_b = getMicrophonePair_DOA_Intersection_linear(micA_pos, micB_pos, micC_pos, micD_pos, steep1, steep2)
estimation = plausibleFilter_TDOA(solutions)
# Kurven zur Darstellung
X_NOL_CURVE1, Y_NOL_CURVE1 = KarstenDOA_calculateCurve_nonlinear(micA_pos, micB_pos, delta_s1, res=0.01, rang=10)
