-
Notifications
You must be signed in to change notification settings - Fork 0
/
Radar_tracking_funcV2.py
194 lines (161 loc) · 7.48 KB
/
Radar_tracking_funcV2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
import math
import copy
#Gegeven informatie vanuit radar
# t = int(parts[0])
# heading = float(parts[1])
# elevation = float(parts[2])
# distance = float(parts[3])
#
# # Compute the distance on the ground plane
# ground_distance = distance * math.cos(math.radians(elevation))
def relativepos(heading, elevation, distance, ground_distance):
#Input zijn de gegevens uit de radar, output is relative position
height = distance * math.sin(math.radians(elevation))
x_relative = math.sin(math.radians(heading)) * ground_distance
y_relative = math.cos(math.radians(heading)) * ground_distance
return [x_relative, y_relative, height]
def posToBase(pos, radarpos):
posTrue = [0,0,0]
for i in [0,1,2]:
posTrue[i] = pos[i] + radarpos[i]
return posTrue
#Het inproduct.
def dot(x, y):
return sum(x_i*y_i for x_i, y_i in zip(x, y))
class Contact():
#Deze class bevat alle informatie over één contact
def __init__(self, heading, elevation, distance, ground_distance, t, ID, radarpos, IFF=None):
#Initeren van de class. Input zijn de radargegevens en de al berekende informatie
#Current stats
self.ID = ID
self.relative_position = relativepos(heading, elevation, distance, ground_distance)
self.heading = heading
self.elevation = elevation
self.distance = distance
self.ground_distance = ground_distance
self.lastUpdate = t
self.posTrue = self.relativeToBase(radarpos)
self.relateToBase()
self.IFF = IFF
if self.IFF:
self.status = 'friendly'
elif [round(self.posTrue[0]),round(self.posTrue[1]),round(self.posTrue[2])] == [-13000,45000,0] or [round(self.posTrue[0]),round(self.posTrue[1]),round(self.posTrue[2])] == [7000,50000,0]:
self.status = 'Patriot radar'
else :
self.status = "Unknown"
#To be calculated
self.speed=list() #vx,vy,vz
self.speedTot = None #totale snelheid
self.acceleration = list() #ax,ay,az
self.accelerationTot = None #totale versnelling
self.cpa = None #Distance to cpa
self.Tcpa = None #Over hoeveel seconden gebeurt cpa
self.futurePosition1 = list() #x,y,z over 1 seconde
self.futurePosition10 = list() #x,y,z over 10 secondes
#Storage old data
self.positionOld = list() #lijst met oude relatieve posities
self.drawPositionOld = list() #lijst met de oude (heading,grounddistance) combinaties
self.speedOld = list()
def update(self, heading, elevation, distance, ground_distance, pos, t, radarpos):
#Het updaten van een contact.
#De input zijn de radargegevens en de al berekende informatie voor het vergelijken.
#Store data
self.positionOld.insert(0,self.posTrue)
self.drawPositionOld.insert(0, (self.heading,self.ground_distance))
if self.speed != list():
self.speedOld = self.speed
#Save current data
self.posTrue= pos #[x,y,z]
self.heading = heading
self.elevation = elevation
self.distance = distance
self.ground_distance = ground_distance
self.relateToBase()
if self.status == "Unknown" and self.distance < 15000:
self.status = "Hostile"
#Calculate information
if self.lastUpdate != t:
self.speed = [ self.posTrue[0] - self.positionOld[0][0] , self.posTrue[1] - self.positionOld[0][1], self.posTrue[2] - self.positionOld[0][2]]
self.speedTot = self.lengte(self.speed)
if self.speedOld != list():
self.acceleration = [ self.speed[0] - self.speedOld[0], self.speed[1] - self.speedOld[1], self.speed[2] - self.speedOld[2]]
self.accelerationTot = self.lengte(self.acceleration)
self.cpa, self.Tcpa = self.cpa_calc()
self.futurePosition1 = self.futurePosition(1)
self.futurePosition10 = self.futurePosition(10)
self.lastUpdate = t
def lengte(self, lijst):
totaal = 0
for i in lijst:
totaal += i**2
return totaal**0.5
def posToBearing(self, pos):
ground_distance = ((pos[0]**2) + (pos[1]**2))**0.5
if round(ground_distance ) == 0:
ground_distance = 1
heading = math.degrees(math.asin(pos[0] / ground_distance))
height = pos[2]
return heading, (ground_distance, height) #heading in degrees, tuple in m
def relateToBase(self):
self.ground_distance = ((self.posTrue[0]**2) + (self.posTrue[1]**2))**0.5
self.heading = math.degrees(math.asin(self.posTrue[0] / self.ground_distance))
self.height = self.posTrue[2]
self.elevation= math.degrees(math.atan(self.height / self.ground_distance))
self.distance = (self.ground_distance**2 + self.height**2)**0.5
def cpa_calc(self):
a = self.posTrue
v = self.speed
if self.speed == [0,0,0]:
return 'n/a', 'n/a'
else:
time = -dot(a,v)/ dot(v,v)
px,py,pz = self.cpa_pos(a,v,time)
distance = ((px**2 ) + (py **2) + (pz **2))**0.5
return distance, time
#berekenen van de positie van de closest point of advance
def cpa_pos(self,a,v,time):
#p= a+ v*t
px = a[0] + v[0] * time
py = a[1] + v[1] * time
pz = a[2] + v[2] * time
return px,py,pz
def futurePosition(self, time):
pos = self.posTrue
speed = self.speed
futpos = [pos[0] + speed[0] * time , pos[1] + speed[1] * time , pos[2] + speed[2] * time]
return futpos
def compare(self, pos):
totDev = 0
if self.futurePosition1 != list():
for i in [0,1,2]:
totDev += (pos[i] - self.futurePosition1[i])**2 #afstand van de posities
return totDev ** 0.5
else:
for i in [0,1,2]:
totDev += (pos[i] - self.posTrue[i])**2 #afstand van de posities
return totDev ** 0.5
def relativeToBase(self, radarpos):
pos = self.relative_position
posTrue = [0,0,0]
for i in [0,1,2]:
posTrue[i] = pos[i] + radarpos[i]
return posTrue
#def __init__(self, heading, elevation, distance, ground_distance, t)
#
# a = Contact(174.525857226826, 7.29180391166557, 3774.99103205393, 3774.99103205393 * math.cos(math.radians(7.29180391166557)), 9000)
#
# # 10000 175.190206882535 8.17267984890556 3705.36757074263
# #def update(self, heading, elevation, distance, ground_distance, pos, t)
#
# x,y,z = relativepos(175.190206882535, 8.17267984890556, 3705.36757074263, 3705.36757074263 * math.cos(math.radians(8.17267984890556)))
# pos = [x,y,z]
# a.update(175.190206882535, 8.17267984890556 , 3705.36757074263, 3705.36757074263 * math.cos(math.radians(8.17267984890556)), pos , 10000 )
#
# #1 11000 175.864192391421 8.96806696301345 3630.61836729314
# bearing = 175.864192391421
# elevation = 8.96806696301345
# distance = 3630.61836729314
# ground_distance = 3630.61836729314 * math.cos(math.radians(8.96806696301345))
# x,y,z = relativepos(175.864192391421, 8.96806696301345, 3630.61836729314, 3630.61836729314 * math.cos(math.radians(8.96806696301345)))
# pos = [x,y,z]
# b = a.compare(pos)