-
Notifications
You must be signed in to change notification settings - Fork 13
/
test_default_blocks.py
113 lines (97 loc) · 3.8 KB
/
test_default_blocks.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
import numpy as np
import unittest
import satellite
import frames
import default_blocks as defblock
from ddt import ddt, data, unpack, file_data
@ddt
class TestDefaultBlocks(unittest.TestCase):
@file_data("test-data/test_defaultsunsensor.json")
@unpack
def test_sunsensor(self,value):
qBI = np.asarray(value[0])
v_sv_i=np.asarray(value[1])
v_pos_i = np.array([1e6,-2.03,-3.0])
v_vel_i = np.array([2.0e3,2.8,-73.2])
qBO = frames.qBI2qBO(qBI,v_pos_i,v_vel_i)
state = np.hstack((qBO,np.zeros(3)))
mySat = satellite.Satellite(state,12.0)
mySat.setQ_BI(qBI)
mySat.setSun_i(v_sv_i)
mySat.setPos(v_pos_i)
mySat.setVel(v_vel_i)
qOI = qnv.quatMultiplyNorm(qnv.quatInv(qBO),qBI)
result = defblock.sunsensor(mySat)
v_expected = value[2];
self.assertTrue(np.allclose(result,v_expected))
@file_data("test-data/test_defaultsunsensor.json")
@unpack
def test_magmeter(self,value):
qBI = np.asarray(value[0])
v_B_i=np.asarray(value[1])
v_pos_i = np.array([1e6,-2.03,-3.0])
v_vel_i = np.array([2.0e3,2.8,-73.2])
qBO = frames.qBI2qBO(qBI,v_pos_i,v_vel_i)
state = np.hstack((qBO,np.zeros(3)))
mySat = satellite.Satellite(state,12.0)
mySat.setQ_BI(qBI)
mySat.setMag_i(v_B_i)
mySat.setPos(v_pos_i)
mySat.setVel(v_vel_i)
qOI = qnv.quatMultiplyNorm(qnv.quatInv(qBO),qBI)
result = defblock.magmeter(mySat)
v_expected = value[2];
self.assertTrue(np.allclose(result,v_expected))
def test_gyroscope(self):
v_w_BI_b = np.array((-3.9999, 4.8575, 0))
self.assertTrue(np.allclose(defblock.gyroscope(v_w_BI_b),v_w_BI_b))
def test_GPS(self):
qBO = np.array([.0,0.,0.,1.])
state = np.hstack((qBO,np.array([-0.1,0.39159385,-0.7])))
sat = satellite.Satellite(state,12.05)
sat.setPos(np.array([7070e3,0.,0.]))
sat.setVel(np.array([5.60,-5.0,0.0]))
v_expected = np.array([7070e3,0.,0.,5.60,-5.0,0.0,12.05])
self.assertTrue(np.allclose(defblock.gps(sat),v_expected))
def test_J2_propagator(self):
qBO = np.array([.0,0.,0.,1.])
state = np.hstack((qBO,np.array([-0.1,0.39159385,-0.7])))
sat = satellite.Satellite(state,12.05)
sat.setPos(np.array([7070e3,0.,0.]))
sat.setVel(np.array([5.60,-5.0,0.0]))
v_expected = np.array([7070e3,0.,0.,5.60,-5.0,0.0,12.05])
self.assertTrue(np.allclose(defblock.J2_propagator(sat),v_expected))
sgp_output = np.genfromtxt('sgp_output.csv',delimiter=",")
defblock.J2_propagator()
defblock.gps()
gps_output= np.genfromtxt('gps_output.csv',delimiter=",")
J2_output = np.genfromtxt('J2_output.csv',delimiter=",")
T = sgp_output[:,0]
l = np.linspace(0,len(T),12,int) #Sample 7 data points from entire file
@data(l[0],l[1],l[2],l[3],l[4],l[5],l[6],l[7],l[8],l[9])
def test_GPS(self,value):
v_expected = self.sgp_output[int(value),1:4]
v_result = self.gps_output[int(value),1:4]
self.assertTrue(np.allclose(v_expected,v_result))
@data(l[0],l[1],l[2],l[3],l[4],l[5],l[6],l[7],l[8],l[9])
def test_J2_propagator(self,value):
v_expected = self.sgp_output[int(value),1:4]
v_result = self.J2_output[int(value),1:4]
self.assertTrue(np.allclose(v_expected,v_result))
def test_controller(self):
qBO = np.array([.0,0.,0.,1.])
state = np.hstack((qBO,np.array([0.10050588,-0.05026119,-0.3014887])))
mySat = satellite.Satellite(state,128.05)
self.assertTrue(np.allclose(defblock.controller(mySat),np.zeros(3)))
def test_disturbance(self):
qBO = np.array([.0,0.,0.,1.])
state = np.hstack((qBO,np.array([0.10050588,-0.05026119,-0.3014887])))
mySat = satellite.Satellite(state,128.05)
self.assertTrue(np.allclose(defblock.disturbance(mySat),np.zeros(3)))
def test_estimator(self):
qBO = np.array([.414,0.5,-0.5,0.])
state = np.hstack((qBO,np.array([0.10050588,-0.05026119,-0.3014887])))
mySat = satellite.Satellite(state,128.05)
self.assertTrue(np.allclose(defblock.estimator(mySat),qBO))
if __name__=='__main__':
unittest.main(verbosity=2)