-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathSatellite.py
121 lines (91 loc) · 3.36 KB
/
Satellite.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
from math import acos, atan2, pi
from EarthValues import M, G
import numpy as np
from igrf import igrf as MagneticFieldModel
import DCM
from datetime import date
# import Magnetometer
# import Gyroscope
# import Filter
from backup_useless_model.Instrument import probe
from typing import List
m: float = 2.6
I = np.diag([0.9, 0.9, 0.3])
invI = np.linalg.inv(I)
def rotationModel(w, torques):
H = I @ w
c = np.cross(w, H, axis=0)
w_dot = invI @ (torques - c)
return w_dot
def attitudeModel(quaternion, w):
assert(quaternion.shape == (4, 1))
assert(w.shape == (3, 1))
p, q, r = w
PQRMAT = np.array([[0, -p, -q, -r], [p, 0, r, -q], [q, -r, 0, p], [r, q, -p, 0]], dtype=float)
quaternion_dot = 0.5*PQRMAT @ quaternion
quaternion_dot = np.reshape(quaternion_dot, (4, 1))
assert(quaternion_dot.shape == (4, 1))
return quaternion_dot
def accelerationModel(position: List[float]):
rho: float = np.linalg.norm(position)
rhat: float = position/rho
Fgrav: list[float] = -(G*M*m/rho**2)*rhat
accel: list[float] = Fgrav/m
return accel
def magneticModel(position, posAngle):
rho: float = np.linalg.norm(position)
phiE, thetaE, psiE = posAngle.flat
latitude: float = 90 - thetaE * (180/pi)
longitude: float = psiE * (180/pi)
rho_km: float = (rho) / 1000
today = date.fromisoformat('2020-01-01')
BNED = MagneticFieldModel(today, glat=latitude, glon=longitude, alt_km=rho_km, itype=2)
BNED = np.array([BNED.north, BNED.east, BNED.down], dtype=float)
assert(BNED.shape == (3, 1))
return BNED
def geocentric(position):
x, y, z = position
rho: float = np.linalg.norm(position)
phiE: float = 0.
thetaE: float = acos(z/rho)
psiE: float = atan2(y, x)
return np.array([[phiE, thetaE, psiE]]).T
def propagateVector(v, pos_angles, q0123):
assert(v.shape == (3, 1))
assert(pos_angles.shape == (3, 1))
BI = DCM.fromEulerAngle(pos_angles) @ v
assert(BI.shape == (3, 1))
body_B = DCM.fromQuaternion(q0123).T @ BI
body_B = body_B*1e-9
return body_B
def Magnetorquer(state):
return np.array([[0, 0, 0]], dtype=float).T
def Model(t: float, state: List[float]):
"""
Returns the change, d state/dt
"""
assert(state.shape == (13, 1))
angular_speed = state[10:13]
quaternion = state[6:10]
quaternion_dot = attitudeModel(quaternion, angular_speed)
position_cartesian = state[0:3]
acceleration = accelerationModel(position_cartesian)
# for output data, not nesscery (line 94 to 108)
# if t % 20 == 0:
# pos_geocentric = geocentric(position_cartesian)
# BNED = magneticModel(position_cartesian, pos_geocentric)
# pos_geocentric[1, 0] = pos_geocentric[1, 0] + pi # TODO: WHY
# Model.BB = propagateVector(BNED, pos_geocentric, quaternion)
# measure_B = Magnetometer.Model(Model.BB)
# measure_omega = Gyroscope.Model(angular_speed)
# [est_B, est_omega] = Filter.Estimate(measure_B, measure_omega)
# probe(est_omega, 1)
# probe(est_B, 2)
# probe(Model.BB)
net_torques = Magnetorquer(state)
angular_acceleration = rotationModel(angular_speed, net_torques)
velocity = state[3:6]
dstate = np.vstack([velocity, acceleration, quaternion_dot, angular_acceleration])
assert(dstate.shape == (13, 1))
return dstate
Model.BB = np.array([[0, 0, 0]], dtype=float).T