TDOA定位
Posted 崔鹏飞
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了TDOA定位相关的知识，希望对你有一定的参考价值。
独立的TDOA程序
import numpy as np
import math
import matplotlib.pyplot as plt
def distance(x1, y1, x2, y2):
dist = math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
return dist
def 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 np
import csv
def 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) 4D
def 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 InputData0
def 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 OutputData0
if __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-9
def 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 t1
def 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,y
def 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)
以上是关于TDOA定位的主要内容，如果未能解决你的问题，请参考以下文章
基于到达时间差（TDOA）的室内定位（/无线传感器网络定位）——极大似然估计ML
目标定位基于matlab TDOA GPS混合定位含Matlab源码 2310期