独立的TDOA程序
import numpy as npimport mathimport matplotlib.pyplot as pltdef distance(x1, y1, x2, y2): dist = math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)) return distdef TdoaPositionAlg(BS,r21,r31, figFlag = False): """ :param x1: BS1.x :param y1: BS1.y :param x2: BS2.x :param y2: BS2.y :param x3: BS3.x :param y3: BS3.y :param r21: distance(BS2,UE) - distance(BS1,UE) :param r31: distance(BS3,UE) - distance(BS1,UE) return: :param x: UE.x :param y: UE.y :return: """ x1,y1 = BS[0][0],BS[0][1] x2,y2 = BS[1][0],BS[1][1] x3,y3 = BS[2][0],BS[2][1] # print(x1,y1,x2,y2,x3,y3) x21 = x2 - x1 x31 = x3 - x1 y21 = y2 - y1 y31 = y3 - y1 # print([x21, x31, y21, y31]) P1_tmp = np.array([[x21, y21], [x31, y31]]) # print("P1_tmp:") # print(P1_tmp) P1 = (-1) * np.linalg.inv(P1_tmp) # print(P1) P2 = np.array([[r21], [r31]]) # print("P2") # print(P2) K1 = x1 * x1 + y1 * y1; K2 = x2 * x2 + y2 * y2; K3 = x3 * x3 + y3 * y3; # print(K1, K2, K3) P3 = np.array([[(-K2 + K1 + r21 * r21) / 2], [(-K3 + K1 + r31 * r31) / 2]]) # print("P3:") # print(P3) tt = np.dot(P1, P2) a = np.dot(tt.T, tt) - 1 X1 = np.array([[x1], [y1]]) # print(x1, tt) b = np.dot(np.dot(P1, P2).T, np.dot(P1, P3) - X1) + np.dot((np.dot(P1, P3) - X1).T, np.dot(P1, P2)) c = np.dot((np.dot(P1, P3) - X1).T, (np.dot(P1, P3) - X1)) val = np.roots([a[0, 0], b[0, 0], c[0, 0]]) print("Roots = ", val) xy1 = (np.dot(P1, P2)) * val[0] + np.dot(P1, P3) print("Using root 0:") print(xy1[0].round(3), xy1[1].round(3)) xy2 = (np.dot(P1, P2)) * val[1] + np.dot(P1, P3) print("Using root 1:") print(xy2[0].round(3), xy2[1].round(3)) if isinstance(val[0], float) and (not isinstance(val[1], float)): nr1 = val[0] elif isinstance(val[1], float) and (not isinstance(val[0], float)): nr1 = val[1] elif isinstance(val[1], float) and (isinstance(val[0], float)): print("Both float") if val[0] < 0 and val[1] > 0: nr1 = val[1] elif val[0] > 0 and val[1] < 0: nr1 = val[0] else: print("[Warning] Unkonw which root is better!") nr1 = val[0] elif not isinstance(val[1], float) and (not isinstance(val[0], float)): print("Both float") if val[0].real < 0 and val[1].real > 0: nr1 = abs(val[0])#val[0].real elif val[0].real > 0 and val[1].real < 0: nr1 = abs(val[1])#val[1].real else: print("[Warning] Unkonw which root is better!") nr1 = abs(val[0])#val[0].real xy_esti = (np.dot(P1, P2)) * nr1 + np.dot(P1, P3) print("Estimated UE coordination:") print(xy_esti[0].round(3), xy_esti[1].round(3)) if figFlag: x,y = xy_esti[0], xy_esti[1] if nr1 == abs(val[0]): xd,yd = xy1[0].round(3), xy1[1].round(3) else: xd, yd = xy2[0].round(3), xy2[1].round(3) plt.scatter(x1, y1, marker="<", label="BS1") plt.scatter(x2, y2, marker="<", label="BS2") plt.scatter(x3, y3, marker="<", label="BS3") plt.scatter(x, y,marker="o", label="Target") plt.scatter(xd, yd,marker="o", label="Discard") plt.legend() plt.plot([x1, x], [y1, y],'--') plt.plot([x2, x], [y2, y],'--') plt.plot([x, x3], [y, y3],'--') plt.show() return round(xy_esti[0][0],3), round(xy_esti[1][0],3)if __name__ == "__main__": x1 = 10 y1 = 10 x2 = 240 y2 = 20 x3 = 124 y3 = 250 x = 110 y = 150 print("Original UE coordinations: ") print(x,y) r1 = distance(x1, y1, x, y) r2 = distance(x2, y2, x, y) r3 = distance(x3, y3, x, y) # print("distance") # print(r1, r2, r3) r21 = r2 - r1 r31 = r3 - r1 # print(r21, r31) [x_e,y_e] = TdoaPositionAlg(x1,y1,x2,y2,x3,y3,r21,r31)
配套的Data类和待修改的main函
import numpy as npimport csvdef str2complex(x): return complex(x.strip().replace("i","j",1))def cs2int(x): return int(x.real)class Data(object): def __init__(self, name = "InputData0"): self.name = name self.NScene = 1 self.data = None self.s = 0 # ctrol flow self.NBS = 3 self.Nant = 64 self.BSxy = [] self.Beacon = {'x':None, 'y':None, 'h':None, 'Nsc':None} self.BeaconH = None # 3*2Nsc*Nant [[[]]] self.NUE = 1 self.UE = [] # UE1,UE2,...Parameter [h,Nsc,NBS, BSidx] self.UEH = [] #NUE * (NBS * 2Nsc * Nant) 4D def parseData(self): data = ReadData(self.name) self.NScene = cs2int(data[0][0]) self.data = data self.s = 1 def parse1Scene(self): s = self.s self.NBS = cs2int(self.data[s][0]) s += 1 for row in self.data[s:(s+3)]:#[2:5]: self.BSxy.append([row[0].real, row[1].real, row[2].real]) s += 3 self.Beacon = {'x':self.data[s][0].real, 'y':self.data[s][1].real, 'h':self.data[s][2].real, 'Nsc': cs2int(self.data[s][3])} self.BeaconH = np.array(np.zeros((3,2*self.Beacon["Nsc"], self.Nant),complex)) s += 1 for ii in range(self.NBS): for jj in range(2*self.Beacon["Nsc"]): self.BeaconH[ii,jj] = np.array(self.data[s]) s += 1 self.NUE = cs2int(self.data[s][0]) s += 1 self.UEH = np.array(np.zeros((self.NUE,self.NBS,2*1632,self.Nant), complex)) for ii in range(self.NUE): print("UE Line: ", s) self.UE.append({'h':self.data[s][0].real, 'Nsc':cs2int(self.data[s][1]), 'M': cs2int(self.data[s][2]),'Idx':[cs2int(x) for x in self.data[s][3:(3+cs2int(self.data[s][2]))] ]}) print("UE ", self.UE[-1]) s += 1 for jj in self.UE[-1]['Idx']: # Idx in [1,2,3] for kk in range(2*self.UE[-1]['Nsc']): self.UEH[ii,jj-1,kk] = np.array(self.data[s]) s += 1 print(s) self.s = s def resetScene(self): self.NBS = 3 self.Nant = 64 self.BSxy = [] self.Beacon = {'x':None, 'y':None, 'h':None, 'Nsc':None} self.BeaconH = None # 3*2Nsc*Nant [[[]]] self.NUE = 1 self.UE = [] # UE1,UE2,...Parameter [h,Nsc,NBS, BSidx] self.UEH = [] #NUE * (NBS * 2Nsc * Nant) 4Ddef ReadData(file = "InputData0", saveFlag = True, printFlag = False): InputData0 = [] with open(file + ".csv", newline='') as csvfile: reader = csv.DictReader(csvfile, fieldnames=list(range(0,64))) for row in reader: x = [] for v in row.values(): if v: x.append(str2complex(v)) InputData0.append(x) if saveFlag: np.save("InputData0", InputData0) if printFlag: for r in InputData0[0:10]: print(r) return InputData0def LoadData(file = "InputData0", flag = False): if flag: print(" <<<<<<<<Print out data>>>>>> ") ReadData0 = np.load("InputData0.npy", allow_pickle = True) if flag: for r in ReadData0[0:10]: print(r)def WriteRst(filename,data): with open(filename + ".csv",'a', newline = '') as csvfile: writer = csv.DictWriter(csvfile,['x','y']) for d in data: writer.writerow({'x':d[0], 'y':d[1]})def ReadRst(filename): OutputData0 = [] with open(filename + ".csv", newline='') as csvfile: reader = csv.DictReader(csvfile, fieldnames=list(range(0,2))) for row in reader: x = [float(row[0]), float(row[1])] OutputData0.append(x) return OutputData0if __name__ == "__main__": # WriteRst('tst', [[2.222332,3.333],[4.55,5.66667]]) # WriteRst('tst', [[2.222332, 3.333], [4.55, 5.66667]]) # WriteRst('tst', [[2.222332, 3.333], [4.55, 5.66667]]) data0 = Data("InputData2") data0.parseData() data0.resetScene() data0.parse1Scene()
main函
from TdoaPositionAlg import *from ReadWriteFiles import *dt = 1e9 / (30 * 2 * 1e3 * 1632)c = 3e8 * 1e-9 #m/s * 1e-9def PDP(h, Nsc, figFlag = False, str = "Beacon"): pdpo = np.fft.ifft(h, Nsc) id = np.argmax(pdpo[0:1000]) t1 = dt * id if figFlag: plt.figure() plt.plot(pdpo[0:50], '-or') plt.title(str + "PDP Profile") plt.show() return t1def CalcTdoa(H, nAnt = 1, NscB = 1632, figFlag = False, nBS = 3, Idx = [1,2,3]): # average all antennas, just 1 simplest way t12 = [] t13 = [] if nBS == 3: for ii in range(nAnt): for a in [0,NscB]: # if isBeacon: t1_b = PDP(H[0,a + 0:NscB,ii], NscB, True) t2_b = PDP(H[1,a + 0:NscB,ii], NscB, True) t3_b = PDP(H[2,a + 0:NscB,ii], NscB, True) # else: # t1_b = PDP(H[UE, 0,0:NscB,ii], NscB, True) # t2_b = PDP(H[UE, 1,0:NscB,ii], NscB, True) # t3_b = PDP(H[UE, 2,0:NscB,ii], NscB, True) t12_b = t1_b - t2_b t13_b = t1_b - t3_b t12.append(t12_b) t13.append(t13_b) elif 2 == nBS: for ii in range(nAnt//2): for a in [0,NscB]: # if isBeacon: print(int(ii + nAnt // 2)) t1_b = PDP(H[Idx[0]-1,a + 0:NscB,ii], NscB, True) t2_b = PDP(H[Idx[1]-1,a + 0:NscB,ii], NscB, True) t3_b = PDP(H[Idx[1]-1,a + 0:NscB,int(ii+nAnt//2)], NscB, True) # else: # t1_b = PDP(H[UE, 0,0:NscB,ii], NscB, True) # t2_b = PDP(H[UE, 1,0:NscB,ii], NscB, True) # t3_b = PDP(H[UE, 2,0:NscB,ii], NscB, True) t12_b = t1_b - t2_b t13_b = t1_b - t3_b t12.append(t12_b) t13.append(t13_b) x,y = np.mean(t12),np.mean(t13) return x,ydef CalcMSE(XY, Base): MSE = [] for ii in range(len(XY)): MSE.append(distance(XY[ii][0],XY[ii][1],Base[ii][0],Base[ii][1])) print("MSE = ", MSE[-1])if __name__ == "__main__": files = ['1','2'] # '0' "InputData0" for f in files: data0 = Data("InputData" + f) data0.parseData() s = 1 for sce in range(data0.NScene): data0.resetScene() data0.parse1Scene() # calc beacon distance print("Beacon Position: ") print(data0.Beacon) d12_b = distance(data0.Beacon['x'],data0.Beacon['y'], data0.BSxy[0][0], data0.BSxy[0][1]) - \ distance(data0.Beacon['x'],data0.Beacon['y'], data0.BSxy[1][0], data0.BSxy[1][1]) d13_b = distance(data0.Beacon['x'],data0.Beacon['y'], data0.BSxy[0][0], data0.BSxy[0][1]) - \ distance(data0.Beacon['x'],data0.Beacon['y'], data0.BSxy[2][0], data0.BSxy[2][1]) t12_b, t13_b = CalcTdoa(data0.BeaconH, 64, 1632, True) [x_b, y_b] = TdoaPositionAlg(data0.BSxy, -t12_b * c, -t13_b * c, True) XY_before = [] XY = [] for u in range(data0.NUE): t12_b, t13_b = CalcTdoa(data0.UEH[u], 64, data0.UE[u]['Nsc'], True, data0.UE[u]['M'], data0.UE[u]['Idx']) print("UE = ", u, ", before") [x_e, y_e] = TdoaPositionAlg(data0.BSxy, -t12_b * c, -t13_b * c, True) XY_before.append([x_e, y_e]) print(x_e, y_e) print("UE = ", u, ", After") [x_e, y_e] = TdoaPositionAlg(data0.BSxy, -(t12_b * c + d12_b), -(t13_b * c + d13_b), True) XY.append([x_e, y_e]) print(x_e, y_e) # save Rsts #WriteRst("OutputData0Rst", XY) WriteRst("OutputData" + f, XY) #0Rst # WriteRst("OutputData0Rst", XY_before) # calc MSE print("<<<<<< Beacon MSE >>>>>>>") CalcMSE([[x_b, y_b]],[[data0.Beacon['x'],data0.Beacon['y']]]) # if '0' == f: Base = ReadRst("OutputData0") print("<<<<<< MSE >>>>>>>") print("Before") CalcMSE(XY_before, Base) print("After") CalcMSE(XY, Base)