TDOA 之TDOA算法python實現

這里指的TDOA算法，實際是解兩個雙曲線方程，由於兩個二次方程設計東西較多，如果強解，計算量很大，從網上參考了如下鏈接:

算法推到:https://blog.csdn.net/lpsl1882/article/details/51519303

Matlab實現:https://blog.csdn.net/chenxy_bwave/article/details/86650983

 

我主要講matlab 相關算法用python再次實現，后期TDOA上位機會基於Python去寫

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
    
x1 = 10
y1 = 10 
x2 = 240 
y2 = 20 
x3 = 124
y3 = 250
x  = 123
y  = 134
# 

print(x1,y1,x2,y2,x3,y3,x,y)

plt.scatter(x1,y1,label="1")
plt.scatter(x2,y2,label="2")
plt.scatter(x3,y3,label="3")
plt.scatter(x,y,label="x")
plt.legend()

# 
plt.plot([x1,x],[y1,y])
plt.plot([x2,x],[y2,y])
plt.plot([x,x3],[y,y3])

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)
 
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)*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)

xy_esti = (np.dot(P1 , P2)) * r1 +np.dot( P1 , P3)

print("xy_esti")
#print(type(xy_esti[0]))
print(int(xy_esti[0]),int(xy_esti[1]))  

　　

運行結果截圖:

標簽節點無論是在三個基站組成的范圍內還是范圍外面，都能正確計算出結果。

以上全部執行print以及繪圖，所有時間開銷為:Time used: 0.0519 秒

將print 和 繪圖去掉，單獨計算坐標解算時間:Time used: 0.0013 秒

測試機器：Win7，CPU：E5-2670