独立的 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)