def test_quaternion_rotation():
########### L/R ##########
q1 = np.array([1, 0, 0, 0])
q2 = np.array([2, -3, 1, 0])
np.testing.assert_allclose(q2, conv.L(q1) @ q2)
np.testing.assert_allclose(q2, conv.R(q1) @ q2)
# (Checked using https://www.vcalc.com/equation/?uuid=ca9f0f2b-7527-11e6-9770-bc764e2038f2)
q1 = np.array(
[-1, 2, 0,
4]) # these are not valid quaternions, but the test is still valid.
q2 = np.array([1, 0, 1, 0])
product = np.array([-1, -2, -1, 6])
np.testing.assert_allclose(product, conv.L(q1) @ q2, atol=tol)
np.testing.assert_allclose(product, conv.R(q2) @ q1, atol=tol)
########## quatrot #########
# Verified against Matlab quatrotate()
vec = np.array([-0.7734, -1.7788, -0.9937])
quat = conv.conj(np.array([1, 0, 1, 0]))
ans = np.array([0.9937, -1.7788, -0.7734])
np.testing.assert_allclose(ans,
conv.quatrot(quat / np.linalg.norm(quat), vec),
atol=tol)
vec = np.array([1, 1, 0])
quat = np.array([1, 0, 1, 0])
ans = np.array([0, 1, -1])
np.testing.assert_allclose(ans,
conv.quatrot(quat / np.linalg.norm(quat), vec),
atol=tol)
def step(self, cmd=np.zeros(3)):
"""
Function: step
Propagates dynamics & models sensors for single step
Inputs:
cmd: commanded magnetic moment, [Am^2 or Nm/T]
Outputs:
meas: spoofed sensor measurements
Comments:
-Simulation state format: [ r [km], q [], v [km/s], w [rad/s] ] (13x1 np array)
-Command format: Torque [Nm]
-r & v are in ECI coordinates, q is the transformation from body to ECI, and w is given in the body frame
"""
#------------------------ Propagate Dynamics --------------------
update_f = lambda t, state: calc_statedot(
t, state, cmd, self.structure, self.environment, self.mag_order)
# sol = solve_ivp(update_f, (self.t, self.t+self.tstep), self.state)
# self.t = sol.t[-1]
# self.state = sol.y[:,-1]
self.t, self.state = rk4_step(update_f, self.t, self.state, self.tstep)
self.state[3:7] = self.state[3:7] / np.linalg.norm(
self.state[3:7]) # normalize the quaternion vector
#------------------------ Calculate Environment -------------------
B_ECI = self.environment.magfield_lookup(
self.state[0:3], self.mag_order
) # Earth's magnetic field isn't fixed in ECI space, it's fixed in ECEF space!!!!
B_body = conv.quatrot(conv.conj(self.state[3:7]), B_ECI)
S_ECI = self.environment.sunVector(self.state[0:3])
S_body = conv.quatrot(conv.conj(self.state[3:7]), S_ECI)
#------------------------ Spoof Sensors -------------------------
# Actuate based on truth for now until magnetometer bias estimation, TRIAD, and MEKF have been implemented and tested
# B_body_noise = B_body
S_body_noise = S_body
# S_body_noise = self.sensors.sunsensor.measure(S_body)
w_body_noise = self.state[10:13]
B_body_noise = self.sensors.magnetometer.measure(B_body)
# w_body_noise = self.sensors.gyroscope.measure(self.state[10:13])
meas = np.r_[B_body_noise, w_body_noise, S_body_noise]
#------------------------ Export Data -------------------------
# TO-DO: output desired variables to text file for later plotting/analysis
self.debug_output = [
B_ECI, B_body,
self.environment.isEclipse(self.state[0:3])
]
File_object = open("./results.txt", "a")
File_object.write(str(self.debug_output) + "\n")
File_object.close()
return meas
def test_conj():
q = np.array([1, 0, 0, 0])
np.testing.assert_allclose(-q, conv.conj(q), atol=tol)
q = conv.quat(np.random.rand(4))
qc = conv.conj(q)
# rotate, then unrotate a vector:
v = np.random.rand(3)
np.testing.assert_allclose(v,
conv.quatrot(qc, conv.quatrot(q, v)),
atol=tol)
# Repeat the test with a 4 indices vector:
v = np.random.rand(4)
np.testing.assert_allclose(v,
conv.quatrot(qc, conv.quatrot(q, v)),
atol=tol)
def helper_test2(r_Earth2Sun, r_sat, q, albedo, atol):
'''r_Earth2Sun version'''
rtol = atol
q = conv.quat(q)
sat2sun_input = conv.quatrot(q, sensors.add(r_Earth2Sun,
r_sat)) #in body frame
meas = sensors.vector2sense(sat2sun_input, r_sat, q, albedo)
sat2sun_out = sensors.sense2vector(meas, r_sat, q, albedo)
return np.testing.assert_allclose(sat2sun_out,
sensors.normalize(sat2sun_input),
atol, rtol)
def vector2sense(sat2sun, r_sat, q_eci2body, albedo=True):
"""
Convert the true sun vector to an array of voltages corresponding to sun sensor measurements
Inputs:
sat2sun: satellite to sun vector in body
r_sat: position of satellite in eci
"""
sat2sun = normalize(sat2sun) #make sure vec is normalized
if albedo:
alb = conv.quatrot(q_eci2body, scale(normalize(r_sat),
0.2)) #body frame
irrad_vec = normalize(add(sat2sun, alb))
else:
irrad_vec = sat2sun
return deltas2measure(irrad_vec)
def sense2vector(meas, r_sat, q_eci2body, albedo=True):
"""
Inputs:
meas: raw measurement values from 6 sun sensors. Arranged: [x, -x, y, -y, z, -z]
r_Earth2Sun: Earth to Sun vector
r_sat: position of satellite in ECI
Outputs:
sat2sun: satellite to sun 3-vector (in body frame)
"""
irrad_vec = [meas[0] - meas[1], meas[2] - meas[3], meas[4] - meas[5]
] #create irradiance vector from sensor values
irrad_vec = normalize(irrad_vec) # normalize irradiance vector
if albedo:
alb = conv.quatrot(q_eci2body, scale(normalize(r_sat),
0.2)) #convert to body frame
sat2sun = normalize(
sub(irrad_vec,
alb)) #vector subt. irradiance vec and albedo vec, normalize
else:
sat2sun = irrad_vec
return sat2sun #in body frame