-
Notifications
You must be signed in to change notification settings - Fork 0
/
equations.py
109 lines (79 loc) · 2.56 KB
/
equations.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
import numpy as np
# flake8: noqa
# constants
n0 = 2 * np.pi / (24 * 60 * 60) # Earth angular's velocity (rad/s)
m = 3000 # mass (kg)
dt = 5 * 60 # dt in seconds
total_time = 32 * 60 * 60 # total time of one week in seconds
num_steps = total_time//dt # iterations
R = 1.5 # radius of the chaser
H = 8 # height of the chaser
J = [[m * ((R ** 2) / 4 + (H ** 2) / 12), 0, 0], [0, m * ((R ** 2) / 4 + (H ** 2) / 12), 0], [0, 0, m * R ** 2 / 2]] # inertia matrix
# we took it as the chaser is a cylinder, homogenous and full of mass
'''@njit
def CW_finite_diff(r0, v0, n, dt, num_steps, F=np.array([0, 0, 0])):
x0, y0, z0 = r0
vx0, vy0, vz0 = v0
Fx, Fy, Fz = F
x, y, z = x0, y0, z0
vx, vy, vz = vx0, vy0, vz0
n2 = n ** 2
dt2 = dt ** 2
for i in range(num_steps):
ax = 3 * n2 * x + 2 * n * vy + Fx / m
ay = -2 * n * vx + Fy / m
az = -n2 * z + Fz / m
x += vx * dt + 0.5 * ax * dt2
y += vy * dt + 0.5 * ay * dt2
z += vz * dt + 0.5 * az * dt2
vx += ax * dt
vy += ay * dt
vz += az * dt
return np.array([x, y, z]), np.array([vx, vy, vz])'''
# discretized clohessy-wiltshire equations
def CW_finite_diff(r0, v0, n, t, F=np.array([0, 0, 0])):
x0, y0, z0 = r0
vx0, vy0, vz0 = v0
Fx, Fy, Fz = F
dt = 1.0
n_iter = int(t/dt)
x, y, z = x0, y0, z0
vx, vy, vz = vx0, vy0, vz0
for i in range(n_iter):
# finite differences
ax = 3*n**2*x + 2*n*vy + Fx/m
ay = -2*n*vx + Fy/m
az = -n**2*z + Fz/m
x += vx*dt + 0.5*ax*dt**2
y += vy*dt + 0.5*ay*dt**2
z += vz*dt + 0.5*az*dt**2
vx += ax*dt
vy += ay*dt
vz += az*dt
return np.array([x, y, z]), np.array([vx, vy, vz])
# discretized evolution of q, the rotation quaternion
def next_q(w, q, dt):
w1, w2, w3 = w
qx, qy, qz, qw = q
# quaternion multiplication matrix
Om = np.array([[-qx, -qy, -qz], [qw, -qz, qy], [qz, qw, -qx], [-qy, qx, qw]])
# finite differences
q += 0.5*np.matmul(Om,w)*dt
# we need to normalize the quaternion at each step
qx, qy, qz, qw = q
q /= np.sqrt(qx**2 + qy**2 + qz**2 + qw**2)
return q
# discretized evolution of w, the angular speed of the chaser
def next_w(w, L, dt):
# finite differences
w += np.matmul(np.linalg.inv(J), L-np.matmul(w, np.matmul(J, w)))*dt
return w
def q_and_w(q0, w0, L, t):
q = q0
w = w0
dt = 1.
n_iter = int(t/dt)
for i in range(n_iter):
q = next_q(w, q, dt)
w = next_w(w, L, dt)
return q, w