c4dynamics 1.0.80__py3-none-any.whl → 1.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Potentially problematic release.
This version of c4dynamics might be problematic. Click here for more details.
- c4dynamics/__init__.py +3 -3
- c4dynamics/body/datapoint.py +357 -54
- c4dynamics/body/rigidbody.py +9 -8
- c4dynamics/detectors/__init__.py +1 -2
- c4dynamics/detectors/yolo3_opencv.py +369 -0
- c4dynamics/detectors/{yolo_tf.py → yolo3_tf.py} +3 -3
- c4dynamics/filters/e_kalman.py +4 -4
- c4dynamics/filters/kalman.py +42 -26
- c4dynamics/sensors/seeker.py +2 -2
- c4dynamics/utils/cprint.py +2 -2
- {c4dynamics-1.0.80.dist-info → c4dynamics-1.1.0.dist-info}/METADATA +1 -1
- c4dynamics-1.1.0.dist-info/RECORD +38 -0
- c4dynamics/detectors/yolo_opencv.py +0 -96
- c4dynamics/eqm/eqm3.py +0 -67
- c4dynamics/eqm/eqm6.py +0 -83
- c4dynamics/rotmat/rotfunc.py +0 -45
- c4dynamics/seekers/__init__.py +0 -21
- c4dynamics/seekers/lineofsight.py +0 -67
- c4dynamics/seekers/radar.py +0 -144
- c4dynamics/src/__init__.py +0 -3
- c4dynamics/src/main/__init__.py +0 -3
- c4dynamics/src/main/py/__init__.py +0 -5
- c4dynamics/utils/params.py +0 -25
- c4dynamics-1.0.80.dist-info/RECORD +0 -48
- /c4dynamics/{src/main/resources → resources}/detectors/yolo/v3/coco.names +0 -0
- /c4dynamics/{src/main/resources → resources}/detectors/yolo/v3/yolov3.cfg +0 -0
- /c4dynamics/{src/main/resources → resources}/detectors/yolo/v3/yolov3.weights +0 -0
- {c4dynamics-1.0.80.dist-info → c4dynamics-1.1.0.dist-info}/WHEEL +0 -0
- {c4dynamics-1.0.80.dist-info → c4dynamics-1.1.0.dist-info}/top_level.txt +0 -0
|
@@ -1,96 +0,0 @@
|
|
|
1
|
-
import os
|
|
2
|
-
import cv2
|
|
3
|
-
import numpy as np
|
|
4
|
-
|
|
5
|
-
|
|
6
|
-
MODEL_SIZE = (416, 416, 3)
|
|
7
|
-
CONFIDENCE_THRESHOLD = 0.5
|
|
8
|
-
NMS_THRESHOLD = 0.5 # .3
|
|
9
|
-
|
|
10
|
-
class yolo:
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
def __init__(self):
|
|
14
|
-
yolov3 = os.path.join('c4dynamics', 'src', 'main', 'resources', 'detectors', 'yolo', 'v3')
|
|
15
|
-
weights_path = os.path.join(yolov3, 'yolov3.weights')
|
|
16
|
-
cfg_path = os.path.join(yolov3, 'yolov3.cfg')
|
|
17
|
-
coconames = os.path.join(yolov3, 'coco.names')
|
|
18
|
-
|
|
19
|
-
self.net = cv2.dnn.readNetFromDarknet(cfg_path, weights_path)
|
|
20
|
-
self.net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV)
|
|
21
|
-
|
|
22
|
-
ln = self.net.getLayerNames()
|
|
23
|
-
self.ln = [ln[i - 1] for i in self.net.getUnconnectedOutLayers()]
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
with open(coconames, 'r') as f:
|
|
27
|
-
self.class_names = f.read().strip().split('\n')
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
def detect(self, frame, t, outfile):
|
|
32
|
-
|
|
33
|
-
#
|
|
34
|
-
# Step 1: Preprocess the Frame
|
|
35
|
-
# - Create a blob (binary large object) from the input frame with the specified dimensions
|
|
36
|
-
# - Normalize pixel values to a range of 0 to 1
|
|
37
|
-
# - Specify the dimensions of the input layer of the YOLO model
|
|
38
|
-
# - Swap Red and Blue channels (BGR to RGB)
|
|
39
|
-
# - Set crop to False to preserve the original aspect ratio
|
|
40
|
-
##
|
|
41
|
-
blob = cv2.dnn.blobFromImage(frame, 1 / 255.0, (MODEL_SIZE[0], MODEL_SIZE[1]), swapRB = True, crop = False)
|
|
42
|
-
|
|
43
|
-
#
|
|
44
|
-
# Step 2: Set Input to the YOLO Model and Perform Forward Pass
|
|
45
|
-
# - Set the blob as the input to the YOLO model
|
|
46
|
-
# - Get the names of the output layers of the model
|
|
47
|
-
# - Perform a forward pass through the model to obtain detections
|
|
48
|
-
##
|
|
49
|
-
self.net.setInput(blob)
|
|
50
|
-
detections = self.net.forward(self.ln)
|
|
51
|
-
|
|
52
|
-
#
|
|
53
|
-
# Step 3: Extract Detected Objects
|
|
54
|
-
# - Iterate through the detected objects in the forward pass results
|
|
55
|
-
# - Filter objects based on confidence threshold
|
|
56
|
-
# - Calculate bounding box coordinates and convert to integers
|
|
57
|
-
# - Append bounding box coordinates and class labels to respective lists
|
|
58
|
-
##
|
|
59
|
-
boxes = []
|
|
60
|
-
confidences = []
|
|
61
|
-
classIDs = []
|
|
62
|
-
h, w = frame.shape[:2]
|
|
63
|
-
|
|
64
|
-
for detection in detections:
|
|
65
|
-
for d in detection:
|
|
66
|
-
|
|
67
|
-
scores = d[5:]
|
|
68
|
-
classID = np.argmax(scores)
|
|
69
|
-
confidence = scores[classID]
|
|
70
|
-
|
|
71
|
-
if scores[classID] > CONFIDENCE_THRESHOLD: # Adjust the confidence threshold as needed
|
|
72
|
-
|
|
73
|
-
box = d[:4] * np.array([w, h, w, h])
|
|
74
|
-
(center_x, center_y, width, height) = box.astype('int')
|
|
75
|
-
|
|
76
|
-
x = int(center_x - (width / 2)) # left edge
|
|
77
|
-
y = int(center_y - (height / 2)) # top edge
|
|
78
|
-
|
|
79
|
-
boxes.append([x, y, int(width), int(height)]) # top left x, top left y, width, height
|
|
80
|
-
confidences.append(float(confidence))
|
|
81
|
-
classIDs.append(classID)
|
|
82
|
-
|
|
83
|
-
indices = cv2.dnn.NMSBoxes(boxes, confidences, CONFIDENCE_THRESHOLD, NMS_THRESHOLD)
|
|
84
|
-
|
|
85
|
-
box_out = []
|
|
86
|
-
class_out = []
|
|
87
|
-
|
|
88
|
-
if len(indices) > 0:
|
|
89
|
-
for i in indices.flatten():
|
|
90
|
-
(x, y) = (boxes[i][0], boxes[i][1])
|
|
91
|
-
(w, h) = (boxes[i][2], boxes[i][3])
|
|
92
|
-
box_out.append([x, y, x + w, y + h])
|
|
93
|
-
class_out.append(self.class_names[classIDs[i]])
|
|
94
|
-
box_out = np.array(box_out)
|
|
95
|
-
|
|
96
|
-
return box_out, class_out
|
c4dynamics/eqm/eqm3.py
DELETED
|
@@ -1,67 +0,0 @@
|
|
|
1
|
-
import numpy as np
|
|
2
|
-
|
|
3
|
-
def eqm3(dp, F):
|
|
4
|
-
'''
|
|
5
|
-
Translational motion derivatives.
|
|
6
|
-
|
|
7
|
-
These equations represent a set of first-order ordinary
|
|
8
|
-
differential equations (ODEs) that describe the motion
|
|
9
|
-
of a datapoint in three-dimensional space under the influence
|
|
10
|
-
of external forces.
|
|
11
|
-
|
|
12
|
-
Parameters
|
|
13
|
-
----------
|
|
14
|
-
dp : datapoint
|
|
15
|
-
C4dynamics' datapoint object for which the
|
|
16
|
-
equations of motion are calculated on.
|
|
17
|
-
F : array_like
|
|
18
|
-
Force vector :math:`[F_x, F_y, F_z]`
|
|
19
|
-
|
|
20
|
-
Returns
|
|
21
|
-
-------
|
|
22
|
-
out : numpy array
|
|
23
|
-
:math:`[dx, dy, dz, dv_x, dv_y, dv_z]`
|
|
24
|
-
6 derivatives of the equations of motion, 3 position derivatives,
|
|
25
|
-
and 3 velocity derivatives.
|
|
26
|
-
|
|
27
|
-
Examples
|
|
28
|
-
--------
|
|
29
|
-
|
|
30
|
-
>>> dp = c4d.datapoint(mass = 10) # mass 10kg
|
|
31
|
-
>>> F = [0, 0, c4d.g_ms2] # g_ms2 = 9.8m/s^2
|
|
32
|
-
>>> c4d.eqm.eqm3(dp, F)
|
|
33
|
-
array([0., 0., 0., 0., 0., -0.980665])
|
|
34
|
-
|
|
35
|
-
Euler integration on the equations of motion of
|
|
36
|
-
mass in a free fall:
|
|
37
|
-
|
|
38
|
-
>>> h0 = 100
|
|
39
|
-
>>> dp = c4d.datapoint(z = h0)
|
|
40
|
-
>>> dt = 1e-2
|
|
41
|
-
>>> while True:
|
|
42
|
-
... if dp.z < 0: break
|
|
43
|
-
... dx = c4d.eqm.eqm3(dp, [0, 0, -c4d.g_ms2])
|
|
44
|
-
... dp.X = dp.X + dx * dt
|
|
45
|
-
... dp.store()
|
|
46
|
-
>>> dp.draw('z')
|
|
47
|
-
|
|
48
|
-
.. figure:: /_static/figures/eqm3_z.png
|
|
49
|
-
|
|
50
|
-
'''
|
|
51
|
-
|
|
52
|
-
dx = dp.vx
|
|
53
|
-
dy = dp.vy
|
|
54
|
-
dz = dp.vz
|
|
55
|
-
|
|
56
|
-
dvx = F[0] / dp.mass
|
|
57
|
-
dvy = F[1] / dp.mass
|
|
58
|
-
dvz = F[2] / dp.mass
|
|
59
|
-
|
|
60
|
-
return np.array([dx, dy, dz, dvx, dvy, dvz])
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
c4dynamics/eqm/eqm6.py
DELETED
|
@@ -1,83 +0,0 @@
|
|
|
1
|
-
# from c4dynamics.params import *
|
|
2
|
-
import numpy as np
|
|
3
|
-
|
|
4
|
-
def eqm6(rb, F, M):
|
|
5
|
-
'''
|
|
6
|
-
Translational and angular motion derivatives.
|
|
7
|
-
|
|
8
|
-
A set of first-order ordinary
|
|
9
|
-
differential equations (ODEs) that describe the motion
|
|
10
|
-
of a rigid body in three-dimensional space under the influence
|
|
11
|
-
of external forces and moments.
|
|
12
|
-
|
|
13
|
-
Parameters
|
|
14
|
-
----------
|
|
15
|
-
rb : rigidbody
|
|
16
|
-
C4dynamics' rigidbody object for which the
|
|
17
|
-
equations of motion are calculated on.
|
|
18
|
-
F : array_like
|
|
19
|
-
Force vector :math:`[F_x, F_y, F_z]`
|
|
20
|
-
M : array_like
|
|
21
|
-
Moments vector :math:`[M_x, M_y, M_z]`
|
|
22
|
-
|
|
23
|
-
Returns
|
|
24
|
-
-------
|
|
25
|
-
out : numpy array
|
|
26
|
-
:math:`[dx, dy, dz, dv_x, dv_y, dv_z, d\\varphi, d\\theta, d\\psi, dp, dq, dr]`
|
|
27
|
-
|
|
28
|
-
12 total derivatives; 6 of translational motion, 6 of rotational motion.
|
|
29
|
-
|
|
30
|
-
Examples
|
|
31
|
-
--------
|
|
32
|
-
Euler integration on the equations of motion of
|
|
33
|
-
a stick fixed at one edge:
|
|
34
|
-
|
|
35
|
-
(mass: 0.5 kg, moment of inertia about y: 0.4 kg*m^2
|
|
36
|
-
, Length: 1m, initial Euler pitch angle: 80 deg (converted to radians)
|
|
37
|
-
)
|
|
38
|
-
|
|
39
|
-
>>> dt = 0.5e-3
|
|
40
|
-
>>> t = np.arange(0, 10, dt)
|
|
41
|
-
>>> length = 1 # metter
|
|
42
|
-
>>> rb = c4d.rigidbody(theta = 80 * c4d.d2r, iyy = 0.4, mass = 0.5)
|
|
43
|
-
>>> for ti in t:
|
|
44
|
-
... tau_g = -rb.mass * c4d.g_ms2 * length / 2 * c4d.cos(rb.theta)
|
|
45
|
-
... dx = c4d.eqm.eqm6(rb, np.zeros(3), [0, tau_g, 0])
|
|
46
|
-
... rb.X = rb.X + dx * dt
|
|
47
|
-
... rb.store(ti)
|
|
48
|
-
>>> rb.draw('theta')
|
|
49
|
-
|
|
50
|
-
.. figure:: /_static/figures/eqm6_theta.png
|
|
51
|
-
|
|
52
|
-
'''
|
|
53
|
-
|
|
54
|
-
#
|
|
55
|
-
# translational motion derivatives
|
|
56
|
-
##
|
|
57
|
-
dx = rb.vx
|
|
58
|
-
dy = rb.vy
|
|
59
|
-
dz = rb.vz
|
|
60
|
-
|
|
61
|
-
dvx = F[0] / rb.mass
|
|
62
|
-
dvy = F[1] / rb.mass
|
|
63
|
-
dvz = F[2] / rb.mass
|
|
64
|
-
|
|
65
|
-
#
|
|
66
|
-
# euler angles derivatives
|
|
67
|
-
##
|
|
68
|
-
dphi = (rb.q * np.sin(rb.phi) + rb.r * np.cos(rb.phi)) * np.tan(rb.theta) + rb.p
|
|
69
|
-
dtheta = rb.q * np.cos(rb.phi) - rb.r * np.sin(rb.phi)
|
|
70
|
-
dpsi = (rb.q * np.sin(rb.phi) + rb.r * np.cos(rb.phi)) / np.cos(rb.theta)
|
|
71
|
-
|
|
72
|
-
#
|
|
73
|
-
# angular motion derivatives
|
|
74
|
-
##
|
|
75
|
-
# dp = (lA - q * r * (izz - iyy)) / ixx
|
|
76
|
-
dp = 0 if rb.ixx == 0 else (M[0] - rb.q * rb.r * (rb.izz - rb.iyy)) / rb.ixx
|
|
77
|
-
dq = 0 if rb.iyy == 0 else (M[1] - rb.p * rb.r * (rb.ixx - rb.izz)) / rb.iyy
|
|
78
|
-
dr = 0 if rb.izz == 0 else (M[2] - rb.p * rb.q * (rb.iyy - rb.ixx)) / rb.izz
|
|
79
|
-
|
|
80
|
-
# 0 1 2 3 4 5 6 7 8 9 10 11
|
|
81
|
-
return np.array([dx, dy, dz, dvx, dvy, dvz, dphi, dtheta, dpsi, dp, dq, dr])
|
|
82
|
-
|
|
83
|
-
|
c4dynamics/rotmat/rotfunc.py
DELETED
|
@@ -1,45 +0,0 @@
|
|
|
1
|
-
import numpy as np
|
|
2
|
-
# import c4dynamics as c4d
|
|
3
|
-
from c4dynamics.utils.params import *
|
|
4
|
-
|
|
5
|
-
|
|
6
|
-
def rotx(a):
|
|
7
|
-
''' rotation about x axis dcm by a radians '''
|
|
8
|
-
return np.array([[1, 0, 0], [0, cos(a), sin(a)], [0, -sin(a), cos(a)]])
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
def roty(a):
|
|
12
|
-
''' rotation about y axis dcm by a radians '''
|
|
13
|
-
return np.array([[cos(a), 0, -sin(a)], [0, 1, 0], [sin(a), 0, cos(a)]])
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
def rotz(a):
|
|
17
|
-
''' rotation about z axis dcm by a radians '''
|
|
18
|
-
return np.array([[cos(a), sin(a), 0], [-sin(a), cos(a), 0], [0, 0, 1]])
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
def dcm321(ax, ay, az):
|
|
22
|
-
'''
|
|
23
|
-
321 dcm
|
|
24
|
-
first rotate about z axis by az radians
|
|
25
|
-
then rotate about y axis by ay radians
|
|
26
|
-
finally rotate about x axis by ax radians
|
|
27
|
-
'''
|
|
28
|
-
return rotx(ax) @ roty(ay) @ rotz(az)
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
def dcm321euler(dcm):
|
|
32
|
-
'''
|
|
33
|
-
dcm321
|
|
34
|
-
see peterschaub 2.3.5 varius euler angle transformations
|
|
35
|
-
|
|
36
|
-
| c(theta)*c(psi) c(theta)*s(psi) -sin(theta) |
|
|
37
|
-
| s(phi)*s(theta)*c(psi)-c(phi)*s(psi) -s(phi)*c(theta)*s(psi)-c(phi)*c(psi) s(phi)*c(theta) |
|
|
38
|
-
| c(phi)*s(theta)*c(psi)+s(phi)*s(psi) c(phi)*s(theta)*s(psi)-s(phi)*c(psi) c(phi)*c(theta) |
|
|
39
|
-
|
|
40
|
-
'''
|
|
41
|
-
|
|
42
|
-
psi = arctan(dcm[0, 1] / dcm[0, 0]) * r2d
|
|
43
|
-
theta = -arctan(dcm[0, 2]) * r2d
|
|
44
|
-
phi = arctan(dcm[1, 2] / dcm[2, 2]) * r2d
|
|
45
|
-
return psi, theta, phi
|
c4dynamics/seekers/__init__.py
DELETED
|
@@ -1,21 +0,0 @@
|
|
|
1
|
-
# seekers:
|
|
2
|
-
# matter:
|
|
3
|
-
# radar
|
|
4
|
-
# laser
|
|
5
|
-
# optic
|
|
6
|
-
# functionallity:
|
|
7
|
-
# altitude radar
|
|
8
|
-
# lineofsight seeker
|
|
9
|
-
|
|
10
|
-
# sensors:
|
|
11
|
-
# imu
|
|
12
|
-
# accelerometers
|
|
13
|
-
# roll gyro
|
|
14
|
-
# rate gyro
|
|
15
|
-
# gps
|
|
16
|
-
# lidar
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
from .radar import radar, dzradar
|
|
20
|
-
from .lineofsight import lineofsight
|
|
21
|
-
|
|
@@ -1,67 +0,0 @@
|
|
|
1
|
-
import numpy as np
|
|
2
|
-
|
|
3
|
-
import c4dynamics as c4d
|
|
4
|
-
|
|
5
|
-
# np.warnings.filterwarnings('ignore', category=np.VisibleDeprecationWarning)
|
|
6
|
-
|
|
7
|
-
class lineofsight:
|
|
8
|
-
"""
|
|
9
|
-
the lineofsight is measure line of sight vector rate
|
|
10
|
-
the seeker head lags the true angular rate.
|
|
11
|
-
this lag is represented by a first order transfer function with time constant tau1
|
|
12
|
-
there are assumed to be delays involved in processing the seeker head angular rate singal.
|
|
13
|
-
the filter delays are represented by a first order transfer function with time constant tau2
|
|
14
|
-
"""
|
|
15
|
-
|
|
16
|
-
tau1 = 0
|
|
17
|
-
tau2 = 0
|
|
18
|
-
dt = 0
|
|
19
|
-
|
|
20
|
-
isideal = False
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
omega = np.array([0, 0, 0]) # truth los rate
|
|
24
|
-
omega_ach = np.array([0, 0, 0]) # achieved los rate after first order filter
|
|
25
|
-
omega_f = np.array([0, 0, 0]) # filtered los rate after first order filter
|
|
26
|
-
|
|
27
|
-
_data = np.zeros((1, 7))
|
|
28
|
-
|
|
29
|
-
def __init__(obj, dt, tau1 = 0.05, tau2 = 0.05, ideal = False): #**kwargs):
|
|
30
|
-
# obj.__dict__.update(kwargs)
|
|
31
|
-
obj.dt = dt
|
|
32
|
-
obj.tau1 = tau1 # tracking loop time constant
|
|
33
|
-
obj.tau2 = tau2 # seeker signal processing time constant
|
|
34
|
-
obj.isideal = ideal
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
def measure(obj, r, v):
|
|
38
|
-
# r: range to target (line of sight vector)
|
|
39
|
-
# v: relative velocity with target
|
|
40
|
-
|
|
41
|
-
#
|
|
42
|
-
# true angular rate of the los vector
|
|
43
|
-
##
|
|
44
|
-
obj.omega = np.cross(r, v) / np.linalg.norm(r)**2
|
|
45
|
-
|
|
46
|
-
#
|
|
47
|
-
# achieved seeker-head angular rate vector
|
|
48
|
-
##
|
|
49
|
-
obj.omega_ach = obj.omega_ach * np.exp(-obj.dt / obj.tau1) + obj.omega * (1 - np.exp(-obj.dt / obj.tau1)) # lag
|
|
50
|
-
|
|
51
|
-
#
|
|
52
|
-
# final processed tracking rate signal vector
|
|
53
|
-
##
|
|
54
|
-
obj.omega_f = obj.omega_f * np.exp(-obj.dt / obj.tau2) + obj.omega_ach * (1 - np.exp(-obj.dt / obj.tau2)) # filter
|
|
55
|
-
|
|
56
|
-
if obj.isideal:
|
|
57
|
-
obj.omega_f = obj.omega_ach = obj.omega
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
return obj.omega_f
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
def store(obj, t = -1):
|
|
64
|
-
obj._data = np.vstack((obj._data
|
|
65
|
-
, np.concatenate(([t], obj.omega, obj.omega_f)))).copy()
|
|
66
|
-
|
|
67
|
-
|
c4dynamics/seekers/radar.py
DELETED
|
@@ -1,144 +0,0 @@
|
|
|
1
|
-
import numpy as np
|
|
2
|
-
|
|
3
|
-
import c4dynamics as c4d
|
|
4
|
-
|
|
5
|
-
# np.warnings.filterwarnings('ignore', category=np.VisibleDeprecationWarning)
|
|
6
|
-
|
|
7
|
-
class dzradar:
|
|
8
|
-
"""
|
|
9
|
-
an electromagnetic radar
|
|
10
|
-
meausres target vertical position.
|
|
11
|
-
vk: noise variance
|
|
12
|
-
"""
|
|
13
|
-
|
|
14
|
-
#
|
|
15
|
-
# state vector
|
|
16
|
-
# r position (in one dimension range)
|
|
17
|
-
# v velocity
|
|
18
|
-
# b ballistic coefficient
|
|
19
|
-
##
|
|
20
|
-
r = 0 # measured range
|
|
21
|
-
# vx = 0 # velocity
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
ts = 0 # 50e-3 # sample time
|
|
25
|
-
q33 = 0
|
|
26
|
-
data = np.array([[0, 0, 0, 0]]) # time, z target, v target, beta target
|
|
27
|
-
|
|
28
|
-
# luenberger gains
|
|
29
|
-
L = 0
|
|
30
|
-
|
|
31
|
-
#
|
|
32
|
-
# seeker errors
|
|
33
|
-
##
|
|
34
|
-
bias = 0 # deg
|
|
35
|
-
sf = 1 # scale factor error -10%
|
|
36
|
-
noise = np.sqrt(500 * c4d.ft2m) # 1 sigma
|
|
37
|
-
# misalignment = 0.01 # deg
|
|
38
|
-
|
|
39
|
-
def __init__(obj, x0, filtype, ts):
|
|
40
|
-
'''
|
|
41
|
-
initial estimate:
|
|
42
|
-
100025 ft 25ft error
|
|
43
|
-
6150 ft/s 150ft/s error
|
|
44
|
-
800 lb/ft^2 300lb/ft^2
|
|
45
|
-
'''
|
|
46
|
-
obj.ts = ts
|
|
47
|
-
if filtype == c4d.filters.filtertype.ex_kalman:
|
|
48
|
-
obj.ifilter = c4d.filters.e_kalman(x0
|
|
49
|
-
, [obj.noise
|
|
50
|
-
, 141.42 * c4d.ft2m
|
|
51
|
-
, 300 * c4d.lbft2kgm]
|
|
52
|
-
, obj.ts)
|
|
53
|
-
elif filtype == c4d.filters.filtertype.luenberger:
|
|
54
|
-
# linear model
|
|
55
|
-
beta0 = x0[2]
|
|
56
|
-
A = np.array([[0, 1, 0], [0, -np.sqrt(2 * 0.0034 * c4d.g / beta0)
|
|
57
|
-
, -c4d.g / beta0], [0, 0, 0]])
|
|
58
|
-
b = np.array([[0], [0], [0]])
|
|
59
|
-
c = np.array([1, 0, 0])
|
|
60
|
-
obj.ifilter = c4d.filters.luenberger(A, b, c)
|
|
61
|
-
elif filtype == c4d.filters.filtertype.lowpass:
|
|
62
|
-
obj.ifilter = c4d.filters.lowpass(.2, obj.ts, x0)
|
|
63
|
-
else:
|
|
64
|
-
print('filter type error')
|
|
65
|
-
|
|
66
|
-
obj.data[0, :] = np.insert(x0, 0, 0)
|
|
67
|
-
|
|
68
|
-
|
|
69
|
-
def measure(obj, x):
|
|
70
|
-
#
|
|
71
|
-
# apply errors
|
|
72
|
-
##
|
|
73
|
-
obj.r = x * obj.sf + obj.bias + obj.noise * np.random.randn(1)
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
def filter(obj, t):
|
|
77
|
-
|
|
78
|
-
# check that t is at time of operation
|
|
79
|
-
|
|
80
|
-
|
|
81
|
-
if (1000 * t) % (1000 * obj.ts) > 1e-6 or t <= 0:
|
|
82
|
-
return
|
|
83
|
-
|
|
84
|
-
|
|
85
|
-
# print(t)
|
|
86
|
-
rho = .0034 * np.exp(-obj.ifilter.x[0, 0] / 22000 / c4d.ft2m)
|
|
87
|
-
f21 = -rho * c4d.g * obj.ifilter.x[1, 0]**2 / 44000 / obj.ifilter.x[2, 0]
|
|
88
|
-
f22 = rho * c4d.g * obj.ifilter.x[1, 0] / obj.ifilter.x[2, 0]
|
|
89
|
-
f23 = -rho * c4d.g * obj.ifilter.x[1, 0]**2 / 2 / obj.ifilter.x[2, 0]**2
|
|
90
|
-
|
|
91
|
-
Phi = np.array([[1, obj.ifilter.tau, 0],
|
|
92
|
-
[f21 * obj.ifilter.tau, 1 + f22 * obj.ifilter.tau, f23 * obj.ifilter.tau],
|
|
93
|
-
[0, 0, 1]])
|
|
94
|
-
|
|
95
|
-
Q = np.array([[0, 0, 0],
|
|
96
|
-
[0, obj.q33 * f23**2 * obj.ifilter.tau**3 / 3, obj.q33 * f23 * obj.ifilter.tau**2 / 2],
|
|
97
|
-
[0, obj.q33 * f23 * obj.ifilter.tau**2 / 2, obj.q33 * obj.ifilter.tau]])
|
|
98
|
-
|
|
99
|
-
f = lambda w: c4d.seekers.dzradar.system_model(w)
|
|
100
|
-
|
|
101
|
-
#
|
|
102
|
-
# prepare luenberger matrices
|
|
103
|
-
##
|
|
104
|
-
|
|
105
|
-
''' predict '''
|
|
106
|
-
obj.ifilter.predict(f, Phi, Q)
|
|
107
|
-
''' correct '''
|
|
108
|
-
x = obj.ifilter.update(f, obj.r)
|
|
109
|
-
|
|
110
|
-
obj.r = x[0]
|
|
111
|
-
#
|
|
112
|
-
# store results
|
|
113
|
-
##
|
|
114
|
-
|
|
115
|
-
# obj.data = np.concatenate((obj.data, np.expand_dims(np.insert(x, 0, t), axis = 0)), axis = 0)
|
|
116
|
-
obj.data = np.vstack((obj.data, np.insert(x, 0, t))).copy()
|
|
117
|
-
|
|
118
|
-
return obj.r
|
|
119
|
-
|
|
120
|
-
|
|
121
|
-
|
|
122
|
-
@staticmethod
|
|
123
|
-
def system_model(x):
|
|
124
|
-
dx = np.zeros((len(x), 1))
|
|
125
|
-
dx[0, 0] = x[1, 0]
|
|
126
|
-
dx[1, 0] = .0034 * np.exp(-x[0, 0].astype('float') / 22000 / c4d.ft2m) * c4d.g * x[1, 0]**2 / 2 / x[2, 0] - c4d.g
|
|
127
|
-
dx[2, 0] = 0
|
|
128
|
-
return dx
|
|
129
|
-
|
|
130
|
-
|
|
131
|
-
class radar:
|
|
132
|
-
# direction radar
|
|
133
|
-
|
|
134
|
-
noisestd = 0
|
|
135
|
-
bias = 0
|
|
136
|
-
sf = 1
|
|
137
|
-
|
|
138
|
-
def __init__(obj, **kwargs):
|
|
139
|
-
obj.__dict__.update(kwargs)
|
|
140
|
-
|
|
141
|
-
def measure(obj, x):
|
|
142
|
-
return x * obj.sf + obj.bias + obj.noisestd * np.random.randn()
|
|
143
|
-
|
|
144
|
-
|
c4dynamics/src/__init__.py
DELETED
c4dynamics/src/main/__init__.py
DELETED
c4dynamics/utils/params.py
DELETED
|
@@ -1,25 +0,0 @@
|
|
|
1
|
-
import numpy as np
|
|
2
|
-
|
|
3
|
-
|
|
4
|
-
''' global quantities '''
|
|
5
|
-
pi = np.pi
|
|
6
|
-
g_ms2 = 9.80665 #
|
|
7
|
-
g_fts2 = 32.2 #
|
|
8
|
-
|
|
9
|
-
''' conversion variables '''
|
|
10
|
-
ft2m = 0.3048 # 1 #
|
|
11
|
-
lbft2kgm = 4.88243 # 47.8803 to include gravity # 1
|
|
12
|
-
r2d = 180 / np.pi
|
|
13
|
-
d2r = np.pi / 180
|
|
14
|
-
|
|
15
|
-
''' math functions '''
|
|
16
|
-
sin = np.sin
|
|
17
|
-
cos = np.cos
|
|
18
|
-
tan = np.tan
|
|
19
|
-
asin = np.arcsin
|
|
20
|
-
acos = np.arccos
|
|
21
|
-
atan = np.arctan
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
@@ -1,48 +0,0 @@
|
|
|
1
|
-
c4dynamics/__init__.py,sha256=y01-O0Oe3G30HeaM9CLzApa6Ui88G4EStLuyQuOP6tw,1515
|
|
2
|
-
c4dynamics/body/__init__.py,sha256=JLUVlqcSCqjMAe9fzcdTNCwtay6FJnJfTNpEtJTiaM8,16
|
|
3
|
-
c4dynamics/body/datapoint.py,sha256=J76GqUTrmlJIpEkyXfmCC2XdisF5RXRZzqrjfyP14E4,29793
|
|
4
|
-
c4dynamics/body/rigidbody.py,sha256=mwXu4VyA_6fX0KYT5hXyYSAmsvIwgBUHBEleaXOaXSY,7626
|
|
5
|
-
c4dynamics/detectors/__init__.py,sha256=hohm-657hR6XCmyjL3qWF4qXr8mfMMMxZoke2AuBi4A,108
|
|
6
|
-
c4dynamics/detectors/yolo_opencv.py,sha256=uax-C-cLZOH2q5DHBjb-sstav2fOQSQrYDn0zHAORwk,3631
|
|
7
|
-
c4dynamics/detectors/yolo_tf.py,sha256=yoUiHUeZaZ0wARL7q2_mE-k_c97HZxYTUj6VAhArpJE,5053
|
|
8
|
-
c4dynamics/eqm/__init__.py,sha256=Lq5AYYI9wTF5aPVV2CePIDdb-aS4SmqC8_5bKsKDxNk,9193
|
|
9
|
-
c4dynamics/eqm/derivs.py,sha256=rnC7Y_IHakA-S1m9WjhvSOlC0ISbGeQCzU7qogos6FI,4150
|
|
10
|
-
c4dynamics/eqm/eqm3.py,sha256=-Z1Mgw-6gbXb-IBp8kzIqTxQv-Qkccw-eCy5g7Sg-ro,1572
|
|
11
|
-
c4dynamics/eqm/eqm6.py,sha256=xpQJweo35Eow4cxCKwqCcjkgKkhQcR2X8JQu-wP0Mwg,2620
|
|
12
|
-
c4dynamics/eqm/integrate.py,sha256=ppVu1y0OYa5GI_KZESPQCM4Oky1fSJNOHwW7glOuA9E,9193
|
|
13
|
-
c4dynamics/filters/__init__.py,sha256=KGgAM_W4SLfv7yw6TfzytruHuwbMLXAlMxLzalgJlb4,643
|
|
14
|
-
c4dynamics/filters/e_kalman.py,sha256=BtV1WHFLq9cs6kzUeSw_CH53BOKIPJDsWdeHFmCXF0s,2423
|
|
15
|
-
c4dynamics/filters/filtertype.py,sha256=9Tbyd_IpRrqXfzs_mLTvDSw3NPxb_Tue_6DyLBZs-ZI,188
|
|
16
|
-
c4dynamics/filters/kalman.py,sha256=2DsOvEKxswukaV_dhaCwqQVmWeLJWwPiFAayFhLzIlo,1086
|
|
17
|
-
c4dynamics/filters/lowpass.py,sha256=Z33-eWaIyFdmWUj2KjEaaD6jznt0bZjC5PsF9H6RvlM,473
|
|
18
|
-
c4dynamics/filters/luenberger.py,sha256=0S5v9tQBLlDb1IWT0nhFx3xG8f03SB_9BhoD_hgjxq8,2295
|
|
19
|
-
c4dynamics/rotmat/__init__.py,sha256=x0BVKfGVlV9uYENxRU3xWyyvKCFbN0-aUjZ4XJi10PE,3376
|
|
20
|
-
c4dynamics/rotmat/rotfunc.py,sha256=s6IdVb0DYLG_wnpJY8rP2zVrFaCpepGgO1nDRNA1Dpo,1408
|
|
21
|
-
c4dynamics/rotmat/rotmat.py,sha256=aRbfBkMlLKFiiZ2cPxYxWQsDJ9lnJlyCcwHHAxBloDU,6052
|
|
22
|
-
c4dynamics/seekers/__init__.py,sha256=QYyobmQdFD1GBCAtrGDnW8TWKucg9-2SBlmABDVtxkI,313
|
|
23
|
-
c4dynamics/seekers/lineofsight.py,sha256=gWxFAC5UAIcWRz2kCpWFeMANKc5woztca8XYccZ90TU,2077
|
|
24
|
-
c4dynamics/seekers/radar.py,sha256=SeAbAb7BlBUEKyRx2_XDAUJqa_Xxw9TaB44YBfT55IY,4021
|
|
25
|
-
c4dynamics/sensors/__init__.py,sha256=EFgElESGRoP0qO_iyh0LeEFYbYnUt0dwDeVtPRRa4e4,654
|
|
26
|
-
c4dynamics/sensors/lineofsight.py,sha256=4pkjXij60BXUkExAR1u3bQ6Dh8ZPsNoW7dynSNldF3E,2081
|
|
27
|
-
c4dynamics/sensors/radar.py,sha256=xfBSryrzGi9PL8hjdFSS-t9wB2sWEFjffYUwSPHlGH8,4052
|
|
28
|
-
c4dynamics/sensors/seeker.py,sha256=dEtJg1CtNp_kUVcuW6BlLZka7bV3ZcPh5t56XoYpuek,8089
|
|
29
|
-
c4dynamics/src/__init__.py,sha256=5SnB4hcbS_fhqWL_1DNf-x1BB6waE5qp8NQv__DCShA,15
|
|
30
|
-
c4dynamics/src/main/__init__.py,sha256=J8E7nduP-GyJm7e2Lr5QwxNG-TtxW15d0LkIjoAnZR8,14
|
|
31
|
-
c4dynamics/src/main/py/__init__.py,sha256=LcFz0vjbcAegqOnpUNXoIpoO75jZrS5hlpzcXTakC8U,37
|
|
32
|
-
c4dynamics/src/main/resources/detectors/yolo/v3/coco.names,sha256=DzbNM53Ej4TWW02YPcDfglnbOo7Kuq6TpSDQGtMP9RA,703
|
|
33
|
-
c4dynamics/src/main/resources/detectors/yolo/v3/yolov3.cfg,sha256=4mlbYgIt7E1oZJzoH3Wi-cKOx1-yPIlDL5RQHNqJ6N8,9129
|
|
34
|
-
c4dynamics/src/main/resources/detectors/yolo/v3/yolov3.weights,sha256=Uj5OaeHQFTk6GwpEHO8dnHZZ4-stfhX3k_Bgohsy8pc,248007048
|
|
35
|
-
c4dynamics/utils/__init__.py,sha256=iRj7jQfEn3Bir_B8VTaeUPfm-X3JbyYYknqgmYYIu1o,155
|
|
36
|
-
c4dynamics/utils/const.py,sha256=mgQu-GCWxLJYwEOrDJzQK01m95t05U69ge8q3RGgZIc,420
|
|
37
|
-
c4dynamics/utils/cprint.py,sha256=Fmr2A7AltKDVmN6BVa948qjZ6vDRGva0YtT5hYus6lc,659
|
|
38
|
-
c4dynamics/utils/gen_gif.py,sha256=eGrAyFLH7jx54SrX3zFAyOa74tlzDC3n6VSw-y8COVg,574
|
|
39
|
-
c4dynamics/utils/gif_tools.py,sha256=CSGDqm2_zIf1n1U8DCUnxg2D-TpvHF-t0r8X827MLWs,3336
|
|
40
|
-
c4dynamics/utils/images_loader.py,sha256=ihMtzy1__BV1gvH1Tz6EDJfahpgGV8hn-61Lhx1x0ws,1568
|
|
41
|
-
c4dynamics/utils/import_subdirs.py,sha256=e7M039yyRAoDEu0rEaE02MJpi_oQIaNIIMXi808gWhU,1796
|
|
42
|
-
c4dynamics/utils/math.py,sha256=esVQQGZST53raCPJNS5m1dBS6yqtbbDKzM0qZ32XQac,798
|
|
43
|
-
c4dynamics/utils/params.py,sha256=4vtSFUccXVeU8QGBt4vQdalakD3-iAvecUsAyQINy3Q,485
|
|
44
|
-
c4dynamics/utils/tictoc.py,sha256=3Bnp8jSvizfwe-djhjNok7eZKw_ACYiRPGnC3GklFn0,189
|
|
45
|
-
c4dynamics-1.0.80.dist-info/METADATA,sha256=YW5Dm18zI1CCZXxEI1PwNAH8uZQBYy8Q30BrI6GhUAc,8964
|
|
46
|
-
c4dynamics-1.0.80.dist-info/WHEEL,sha256=yQN5g4mg4AybRjkgi-9yy4iQEFibGQmlz78Pik5Or-A,92
|
|
47
|
-
c4dynamics-1.0.80.dist-info/top_level.txt,sha256=Pp3jXXljS2HbUDGItCnMueDc-7dI8pYDObXNtBhZVG0,11
|
|
48
|
-
c4dynamics-1.0.80.dist-info/RECORD,,
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|