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