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
187
188
189
190
191
192
193
|
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Tue Mar 20 21:18:28 2012
@author: Bernhard Tittelbach, Othmar Gsenger
"""
import numpy as np
import pylab as pl
import scipy.io
import re
w_x = np.random.ranf() * np.pi/2 #rad/s
w_y = np.random.ranf() * np.pi/2 #rad/s
w_z = np.random.ranf() * np.pi/2 #rad/s
filename_export_mat = "./pd_sim.mat"
filename_export_csv = "./pd_sim.csv"
sun = [1,1,1]
sun_intensity = 1.0
earth = [0,-1,-1]
earth_reflection_intensity = 0.10 * sun_intensity # 0 to turn earth off ;-)
moon_reflection_intensity = 0.07 * sun_intensity # 0 to turn moon off ;-)
orbit_height = 310e3 #m
numsamples = 1000
numperiods = 8
sensors = np.matrix([
[ 1,0,0],
[ -1,0,0],
[0,1,0],
[0,-1,0],
[0,0,1],
[0,0,-1]
])
#calculate simduration so we see numperiods periods of the smalest angular speed
simduration = numperiods*np.pi / min(filter(lambda x: x>0.0,[w_x,w_y,w_z])) if w_x or w_y or w_z else 1.0 #in seconds
simtimestep = simduration / numsamples #s
#calculate angle between normalvector of earthdisc and edge of earthdisc as seen from satellite
earth_radius = 6e6 #m
earth_distance = earth_radius + orbit_height #m
angle_earthdisc_visible = np.arcsin(float(earth_radius) / earth_distance )
assert(angle_earthdisc_visible < np.pi/2.0)
w_xyz = np.matrix([w_x,w_y,w_z])
simtimeline = np.arange(0,simduration,simtimestep)
angular_speed_sum_vectors = ( w_xyz.transpose() * simtimeline ).transpose() % (2 *np.pi)
sun = sun / np.linalg.norm(sun)
earth = earth / np.linalg.norm(earth)
angle_sun_earth = np.arccos(max(min(np.dot(sun,earth),1.0),-1.0))
#print angle_sun_earth /np.pi*180, angle_earthdisc_visible /np.pi*180
print "Simulation Input:"
print " Duration:", simduration
print " Step Size:", simtimestep
print " Angular Speed x-axis [rad/s]:", w_x
print " Angular Speed y-axis [rad/s]:", w_y
print " Angular Speed z-axis [rad/s]:", w_z
print " Earth " + ("Light intensity: %f" % earth_reflection_intensity if earth_reflection_intensity else "is switched OFF")
print " Sphere surface, covered by Earth as seen from satellite: %.2f %%" % ((earth_radius**2) / (4.0 * earth_distance**2) * 100.0)
print " Satellite " + (u"Is (!) " if angle_sun_earth < angle_earthdisc_visible else u"is not") + u" in Earth's shadow (Angle Sun-Earth: %d°)" % (angle_sun_earth /np.pi*180.0)
print ""
def getRotationMatrix(angles):
x_angle = angles[0,0]
y_angle = angles[0,1]
z_angle = angles[0,2]
return np.matrix([
[1, 0, 0],
[0, np.cos(x_angle), -np.sin(x_angle)],
[0, np.sin(x_angle), np.cos(x_angle)]
]) * np.matrix([
[ np.cos(y_angle),0,np.sin(y_angle)],
[ 0, 1,0],
[-np.sin(y_angle),0,np.cos(y_angle)]
]) * np.matrix([
[np.cos(z_angle),-np.sin(z_angle),0],
[np.sin(z_angle), np.cos(z_angle),0],
[0,0,1]
])
def rotateVector(vec_to_rotate, angles_vector):
return vec_to_rotate * getRotationMatrix(angles_vector)
def sensor_model_rel_sensitivity_cos(angle):
if angle < np.pi/2.0:
return np.cos(angle)
else:
return 0.0
sensor_model = sensor_model_rel_sensitivity_cos
def sun_earth_shadow(angle):
global angle_sun_earth, angle_earthdisc_visible, sensor_model
if angle_sun_earth < angle_earthdisc_visible:
return 0.0
else:
return sensor_model(angle)
sun_model = sun_earth_shadow
def earthdisc_light_rel_intensity(angle):
global angle_sun_earth, angle_earthdisc_visible, sensor_model
if angle_sun_earth < angle_earthdisc_visible:
return 0.0
#todo: factor in angle of actually lightened surface depending on angle_sun_earth and the angle between sat orbit and the sun
elif angle < angle_earthdisc_visible:
return 1.0
else:
return sensor_model(angle - angle_earthdisc_visible)
earth_model = earthdisc_light_rel_intensity
def _quote(str):
return '"'+ re.sub(r'(?!\\)"','\\"',str) +'"'
def exportCSV(dst_file_path, names, tline, smatrix):
with open(dst_file_path, "w") as fh:
fh.truncate()
fh.write(";".join(map(_quote, ["SimTime"]+names)) +"\n")
for row in np.vstack((tline,smatrix.T)).T:
fh.write(";".join(["%f" % x for x in row.T]) +"\n")
senstrace=[]
suntrace=[]
earthtrace=[]
for cur_rot in angular_speed_sum_vectors:
rot_sensors=rotateVector(sensors,cur_rot)
#sinphi=np.cross(sens1,sun) # /1 / 1
angles_to_sun=np.array(np.arccos(np.dot(rot_sensors,np.matrix(sun).T))).flatten()
angles_to_earth=np.array(np.arccos(np.dot(rot_sensors,np.matrix(earth).T))).flatten()
#print [sensor_rel_sensitivity_cos(a) for a in angle_sun]
light_intensity_per_sensor = \
sun_intensity * np.array([sun_model(a) for a in angles_to_sun]) + \
earth_reflection_intensity * np.array([earth_model(a) for a in angles_to_earth])
senstrace.append(light_intensity_per_sensor)
suntrace.append(angles_to_sun)
earthtrace.append(angles_to_earth)
senstrace = np.matrix(senstrace)
suntrace = np.matrix(suntrace)
earthtrace = np.matrix(earthtrace)
legend_names=["Sensor%d %s" % (x+1, sensors[x].flatten()) for x in range(0,6)]
print "Saving to " + filename_export_mat
scipy.io.savemat(filename_export_mat, mdict={"legend":legend_names, "senstrace":senstrace,"simtimeline":simtimeline,"angular_speed_xyz":w_xyz}, oned_as="column")
print "Saving to " + filename_export_csv
exportCSV(filename_export_csv, legend_names, simtimeline, senstrace)
xlabelpos = np.hstack((np.arange(0,simduration, simduration/5),simduration))
xlabelvalues = ["%.3f" % t for t in xlabelpos]
ylabelpos = [0,0.5,1]
ylabelvalues = [u"0%", u"50%", u"100%"]
#~ pl.figure()
#~ pl.plot(simtimeline, senstrace)
#~ pl.xticks(xlabelpos,xlabelvalues,rotation=0)
#~ pl.yticks(ylabelpos, ylabelvalues)
#~ pl.xlabel("[s]")
#~ pl.ylabel("[rad]")
#~ pl.legend(legend_names)
pl.figure()
for f in range(0,3):
fs = 2*f
fe = fs + 2
pl.subplot(3,1,f+1)
pl.plot(simtimeline, senstrace[:,fs:fe])
pl.xticks(xlabelpos,xlabelvalues,rotation=0)
pl.yticks(ylabelpos, ylabelvalues)
pl.xlabel("[s]")
pl.ylabel("[Light Intensity]")
pl.legend(legend_names[fs:fe])
pl.figure()
spncols=2
spnrows=3
for s in range(0,6):
pl.subplot(spnrows,spncols,s+1)
pl.plot(simtimeline, senstrace[:,s],"g")
pl.plot(simtimeline, 1/(2*np.pi)*suntrace[:,s],"y")
pl.plot(simtimeline, 1/(2*np.pi)*earthtrace[:,s],"b")
pl.xticks(xlabelpos,xlabelvalues,rotation=0)
pl.yticks(ylabelpos, ylabelvalues)
pl.ylabel("[Light Intensity]")
pl.title(legend_names[s])
pl.legend(["Sensor Value","Sun Angle", "Earth Angle"])
pl.show()
|
