Jag skrev ihop ett litet python program för att testa hjärnan lite....
TDOA med succesiv approximation.
Den slumpar ut en koordinat (grön punkt), räknar ut TDOA och skickar in dom till en loop som söker det bästa matchningen och centrumpunkten för den (rosa ring).
Den använder omkretsen på alla 4 korsningspunkterna (vita ringar) för att bedöma vad som är bättre och sämre.
Vill ni "förstöra" genom att lägga in ett fel så gör det på en eller flera av td1,2,3,4.
(Jag gjorde även ett test med en 240MHz dubbelkärnig esp32 men det resultatet kommer senare)
Kod: Markera allt
from cmath import sqrt
import cv2
import numpy as np
import math
import random
def intersection(x1,y1,t1,x2,y2,t2,xmin,xmax,ymin,ymax) :
dx= x2-x1
dy= y2-y1
d=sqrt(dx**2+dy**2).real
if d>(t1+t2) :
return xmin-1, ymin-1
a = (t1**2 - t2**2 + d**2)/(2.0*d)
x3 =x1 + (dx * a/d)
y3 =y1 + (dy * a/d)
h= sqrt(t1**2-a**2).real
rx = -dy * (h/d)
ry = dx * (h/d)
xi = x3 + rx
yi = y3 + ry
if xi>=xmin and xi<=xmax and yi>=ymin and yi<=ymax :
return xi, yi
xi = x3 - rx
yi = y3 - ry
if xi>=xmin and xi<=xmax and yi>=ymin and yi<=ymax :
return xi, yi
return xmin-1, ymin-1
y_size_window=1000
x_size_window = 1000
x=random.randint(0,x_size_window)
y=random.randint(0,y_size_window)
x1=0
y1=y_size_window
x2=x_size_window
y2=y_size_window
x3=x_size_window
y3=0
x4=0
y4=0
t=0
x=509
y=117
font = cv2.FONT_HERSHEY_SIMPLEX
prev_dist=9999999.9
t_testdist=3.0
dist=prev_dist
while(True):
t1=sqrt(x**2+(y_size_window-y)**2).real
t2=sqrt((x_size_window-x)**2+(y_size_window-y)**2).real
t3=sqrt((x_size_window-x)**2+y**2).real
t4=sqrt(x**2+y**2).real
t_calc=min(t1,t2,t3,t4)
td1=t1-t_calc
td2=t2-t_calc
td3=t3-t_calc
td4=t4-t_calc
t12_min=(1000-td1-td2)/2
t23_min=(1000-td2-td3)/2
t34_min=(1000-td3-td4)/2
t41_min=(1000-td4-td1)/2
t=max(t,t12_min,t23_min,t34_min,t41_min)
img = np.zeros((x_size_window,y_size_window,3), np.uint8)
cv2.circle(img,(x1,y1), 5, (0,0,255), -1)
cv2.circle(img,(x2,y2), 5, (255,255,0), -1)
cv2.circle(img,(x3,y3), 5, (255,0,255), -1)
cv2.circle(img,(x4,y4), 5, (0,255,255), -1)
cv2.putText(img,'1',(30,x_size_window-30), font, 1,(0,0,255),2,cv2.LINE_AA)
cv2.putText(img,'2',(y_size_window-30,x_size_window-30), font, 1,(255,255,0),2,cv2.LINE_AA)
cv2.putText(img,'3',(y_size_window-30,30), font, 1,(255,0,255),2,cv2.LINE_AA)
cv2.putText(img,'4',(30,30), font, 1,(0,255,255),2,cv2.LINE_AA)
cv2.circle(img,(x1,y1), int(t+td1), (0,0,255), 1)
cv2.circle(img,(x2,y2), int(t+td2), (255,255,0), 1)
cv2.circle(img,(x3,y3), int(t+td3), (255,0,255), 1)
cv2.circle(img,(x4,y4), int(t+td4), (0,255,255), 1)
x12_x, x12_y=intersection(x1,y1,td1+t,x2,y2,td2+t,0,x_size_window,0,y_size_window)
x23_x, x23_y=intersection(x3,y3,td3+t,x2,y2,td2+t,0,x_size_window,0,y_size_window)
x34_x, x34_y=intersection(x3,y3,td3+t,x4,y4,td4+t,0,x_size_window,0,y_size_window)
x41_x, x41_y=intersection(x1,y1,td1+t,x4,y4,td4+t,0,x_size_window,0,y_size_window)
if x12_x>=0 :
cv2.circle(img,(int(x12_x),int(x12_y)), 5, (255,255,255), 1)
if x23_x>=0 :
cv2.circle(img,(int(x23_x),int(x23_y)), 5, (255,255,255), 1)
if x34_x>=0 :
cv2.circle(img,(int(x34_x),int(x34_y)), 5, (255,255,255), 1)
if x41_x>=0 :
cv2.circle(img,(int(x41_x),int(x41_y)), 5, (255,255,255), 1)
if x12_x>=0 and x23_x>=0 and x34_x>=0 and x41_x>=0:
x_avg=(x12_x+x23_x+x34_x+x41_x)/4
y_avg=(x12_y+x23_y+x34_y+x41_y)/4
cv2.circle(img,(int(x_avg),int(y_avg)), 5, (127,127,255), 1)
dist_12_23=sqrt((x12_x-x23_x)**2+(x12_y-x23_y)**2).real
dist_23_34=sqrt((x34_x-x23_x)**2+(x34_y-x23_y)**2).real
dist_34_41=sqrt((x34_x-x41_x)**2+(x34_y-x41_y)**2).real
dist_41_12=sqrt((x12_x-x41_x)**2+(x12_y-x41_y)**2).real
dist=dist_12_23+dist_23_34+dist_34_41+dist_41_12
print("Dist: ",dist)
if prev_dist<dist:
t_testdist=-t_testdist/10
cv2.circle(img,(x,y), 2, (0,255,0), -1)
t+=t_testdist
prev_dist=dist
cv2.imshow('image', img)
key =cv2.waitKey(50)
if key>=0:
if key& 0xFF == ord('r'): #new dot
x=random.randint(0,x_size_window)
y=random.randint(0,y_size_window)
t=0
prev_dist=9999999.9
dist=prev_dist
t_testdist=3.0
if key& 0xFF == ord('q'):
break
cv2.destroyAllWindows()
