-
Notifications
You must be signed in to change notification settings - Fork 2
/
toolbox.py
186 lines (139 loc) · 4.59 KB
/
toolbox.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
import numpy as np
from numpy import sin, cos, arctan, arccos
"""
Mission scenario and constants
"""
### Mars
# Mars rotational period is 1 day and 37 minutes
R_MARS = 3396.19 # km - Radius
MI_MARS = 42828.3 # km^3/s^2 - Gravity constant
### Nano-sat (LMO) - Circular
h = 400 # Altitude
THETA_DOT_LMO = 0.000884797 # rad/sec - Constant orbit rate
R_LMO = R_MARS + h # km - Orbit radius
### Mothercraft (GMO) - Circular
# Has the sabe rotational period than Mars
R_GMO = 20424.2 # km - Orbit radius
THETA_DOT_GMO = 0.0000709003 # rad/sec - Constant orbit rate
"""
Initial conditions - Orbits and Frames
"""
#### LMO ####
OMEGA_LMO = 20
I_LMO = 30
THETA_LMO_T0 = 60
# Initial LMO orbit position
LMO_EA_T0 = np.array([OMEGA_LMO, I_LMO, THETA_LMO_T0]) # Initial Euler Angles in degrees
LMO_EA_RATE = np.array([0, 0, THETA_DOT_LMO]) # Constant velocity, in rad
LMO_R = np.array([R_LMO, 0, 0]).T # Constant position vector
R_DOT_NANO = np.array([0, R_LMO * THETA_DOT_LMO, 0]) # Velocity in km/s
#### GMO ####
OMEGA_GMO = 0
I_GMO = 0
THETA_GMO_T0 = 250
# Initial GMO orbit position
GMO_EA_T0 = np.array([OMEGA_GMO, I_GMO, THETA_GMO_T0]) # Initial Euler Angles in degrees
GMO_EA_RATE = np.array([0, 0, THETA_DOT_GMO]) # Constant velocity, in rad
GMO_R = np.array([R_GMO, 0, 0]) # Constant position vector
R_DOT_MOTHER = np.array([0, R_GMO * THETA_DOT_GMO, 0]) # Velocity in km/s
#### Sun Reference Frame ####
OMEGA_SUN = 180 # Rotation in 3rd frame
I_SUN = 90 # Rotation in 1st frame
SUN_FRAME = np.array([OMEGA_SUN, I_SUN, 0])
#### Nadir-Pointing Reference Frame ####
## Nadir to Hill frame
OMEGA_NADIR = 180
I_NADIR = 180
THETA_NADIR = 0
NADIR_FRAME = np.array([OMEGA_NADIR, I_NADIR, THETA_NADIR]) # Initial Euler Angles in degrees
# Nadir frame position
w_NADIR = np.array([0, 0, THETA_DOT_LMO]) # rad/sec
"""
Initial conditions - Nano-sat
"""
sigma_BN_T0 = np.array([0.3, -0.4, 0.5]).T # Initial MRP
omega_BN_T0 = np.deg2rad([1.00, 1.75, -2.20]) # rad/s - Initial body angular velocity
# Intertia Tensor
I_nano = np.array([10, 5, 7.5]) * np.eye(3) # kg . m^2
"""
AUXILIARY FUNCTIONS
"""
def EAtoDCM313(t, radian=False):
"""Insert 3-1-3 Euler angles in degrees and returns equivalent DCM"""
if not radian:
t = np.deg2rad(t)
a11 = cos(t[2])*cos(t[0]) - sin(t[2])*cos(t[1])*sin(t[0])
a12 = cos(t[2])*sin(t[0]) + sin(t[2])*cos(t[1])*cos(t[0])
a13 = sin(t[2])*sin(t[1])
a21 = -sin(t[2])*cos(t[0]) - cos(t[2])*cos(t[1])*sin(t[0])
a22 = -sin(t[2])*sin(t[0]) + cos(t[2])*cos(t[1])*cos(t[0])
a23 = cos(t[2])*sin(t[1])
a31 = sin(t[1])*sin(t[0])
a32 = -sin(t[1])*cos(t[0])
a33 = cos(t[1])
DCM = np.array([[a11, a12, a13],
[a21, a22, a23],
[a31, a32, a33]])
return DCM
def DCMtoEA313(DCM, radian=False):
"""
Insert DCM and returns Euler Angles.
radian: True, returns radians. False, returns degrees
"""
t1 = arctan(DCM[2][0]/(-DCM[2][1]))
t2 = arccos(DCM[2][2])
t3 = arctan(DCM[0][2]/DCM[1][2])
EA = np.array([t1, t2, t3])
if radian:
return EA
else:
return np.rad2deg(EA)
def EArate313(t, w, radian_in=False, radian_out=False):
"""
Calculation of 3-1-3 Euler Angle Rates in a given instant.
Angular velocities should be inserted in rad/sec.
radian_in: True if Euler Angles vector are in radian, else false.
radian_out: True if it is desired that the function returns the rates in radians, else false.
"""
if not radian_in:
t = np.deg2rad(t)
a11 = sin(t[2])
a12 = cos(t[2])
a13 = 0
a21 = cos(t[2])*sin(t[1])
a22 = -sin(t[2])*sin(t[1])
a23 = 0
a31 = -sin(t[2])*cos(t[1])
a32 = -cos(t[2])*cos(t[1])
a33 = sin(t[1])
B = np.array([[a11, a12, a13],
[a21, a22, a23],
[a31, a32, a33]])
rate = (1/sin(t[1])) * np.matmul(B, w.T)
if radian_out:
return rate
else:
return np.rad2deg(rate)
def orbit_integrator(orbit, time, step=1):
""""
Integrator for LMO and GMO orbits. The parameter orbit can only be 'LMO' or 'GMO'.
Input time and step in seconds.
"""
history = []
if orbit == 'LMO':
x0 = LMO_EA_T0
for t in range(time):
x = x0 + step * EArate313(x0, LMO_EA_RATE)
history.append(x)
x0 = x
elif orbit == 'GMO':
y0 = GMO_EA_T0
for t in range(time):
y = y0 + step * np.rad2deg(GMO_EA_RATE)
history.append(y)
y0 = y
else:
return False
moment = time // step
matrix = EAtoDCM313(history[moment - 1])
return matrix