def test_rotm2quat(self, value): q = np.asarray(value[3]) m1 = np.asarray(value[0]) m2 = np.asarray(value[1]) m3 = np.asarray(value[2]) A = np.vstack([m1, m2, m3]) qo = qnv.rotm2quat(A) self.assertTrue(np.allclose(qo, q))
def qBO2qBI(v_q_BO, v_pos_i, v_vel_i): #input: Unit quaternion which rotates orbit frame vector to body frame, position and velocity in ecif #output: Unit quaternion which rotates ecif vector to body frame #Orbit frame def: z_o = nadir(opposite to satellite position vector) y_o: cross(v,r) x_o: cross(y,z) z = -v_pos_i / np.linalg.norm(v_pos_i) y = np.cross(v_vel_i, v_pos_i) / np.linalg.norm(np.cross(v_vel_i, v_pos_i)) x = np.cross(y, z) / np.linalg.norm(np.cross(y, z)) m_DCM_OI = np.array([x, y, z]) v_q_OI = qnv.rotm2quat(m_DCM_OI) v_q_BI = qnv.quatMultiplyNorm(v_q_BO, v_q_OI) if v_q_BI[3] < 0.: v_q_BI = -1. * v_q_BI.copy() v_qBI = v_q_BI / np.linalg.norm(v_q_BI.copy()) return v_q_BI
def qBO2qBI(v_q_BO,v_pos_i,v_vel_i): #input: Quaternion corresponding to transformation of components of vector in orbit frame to components in body frame, # position and velocity in ecif #output: Quaternion corresponding to transformation of components of vector in ecif to components in body frame #Orbit frame def: z_o = nadir(opposite to satellite position vector) y_o: cross(v,r) x_o: cross(y,z) z = -v_pos_i/np.linalg.norm(v_pos_i) y = np.cross(v_vel_i,v_pos_i)/np.linalg.norm(np.cross(v_vel_i,v_pos_i)) x = np.cross(y,z)/np.linalg.norm(np.cross(y,z)) m_DCM_OI = np.array([x,y,z]) v_q_OI = qnv.rotm2quat(m_DCM_OI) v_q_BI = qnv.quatMultiplyNorm(v_q_BO,v_q_OI) if v_q_BI[3] < 0. : v_q_BI = -1.*v_q_BI.copy() v_qBI = v_q_BI/np.linalg.norm(v_q_BI.copy()) return v_q_BI
def qBI2qBO(v_q_BI, v_pos_i, v_vel_i): #input: unit quaternion which rotates ecif vector to body frame, position and velocity in ecif #output: unit quaternion which rotates orbit frame vector to body frame #Orbit frame def: z_o = nadir(opposite to satellite position vector) y_o: cross(v,r) x_o: cross(y,z) z = -v_pos_i / np.linalg.norm(v_pos_i) y = np.cross(v_vel_i, v_pos_i) / np.linalg.norm(np.cross(v_vel_i, v_pos_i)) x = np.cross(y, z) / np.linalg.norm(np.cross(y, z)) m_DCM_OI = np.array([x, y, z]) v_q_IO = qnv.rotm2quat(np.transpose(m_DCM_OI)) m_G1 = np.array([[v_q_BI[0], -v_q_BI[1], -v_q_BI[2], -v_q_BI[3]], [v_q_BI[1], v_q_BI[0], v_q_BI[3], -v_q_BI[2]], [v_q_BI[2], -v_q_BI[3], v_q_BI[0], v_q_BI[1]], [v_q_BI[3], v_q_BI[2], -v_q_BI[1], v_q_BI[0]]]) v_q_BO = np.dot(m_G1, v_q_IO) if v_q_BO[0] < 0.: v_q_BO = -1. * v_q_BO.copy() v_q_BO = v_q_BO / np.linalg.norm(v_q_BO.copy()) return v_q_BO
x1 = p.copy() #print x1, k1 x1 = x1 + (k1 + 2 * k2 + 2 * k3 + k4) / 6 return x1 theta0 = 0 pos0 = np.array([[R + 600e3], [0.], [0.]]) dist0 = np.linalg.norm(pos0) v0 = np.array([[0], [sqrt(G * M / dist0)], [0]]) w0 = np.array([[0], [-1 * sqrt(G * M / dist0**3)], [0]]) w0n = np.linalg.norm(w0) v_L_b = np.array([[0.], [0.], [1.]]) m_eu0 = np.array([[0, 0, -1], [1, 0, 0], [0, -1, 0]]) q0 = qnv.rotm2quat(m_eu0) q0 = q0.reshape((4, 1)) time_i = 0 time_f = 10 * pi / w0n step_size = 0.1 nT = int((time_f - time_i) / step_size) dot = np.zeros(nT + 1) dot[0] = np.dot((qnv.quatRotate(q0, v_L_b)).T, pos0) / (np.linalg.norm(pos0)) r = np.zeros(nT + 1) r[0] = np.linalg.norm(pos0) q1 = q0 theta1 = theta0 pos1 = pos0 s1 = np.vstack((pos0, v0))
Izz = .15858572070 Ixy = .00071033134 Iyz = .00240388659 Ixz = .00059844292 m_Inertia = np.array([[Ixx, -1 * Ixy, -1 * Ixz], [-1 * Ixy, Iyy, -1 * Iyz], [-1 * Ixz, -1 * Iyz, Izz]]) m_Inertia_inv = np.linalg.inv(m_Inertia) nLb = 1. nLg = 1. Ms = 10. mu_m = 0 * 0.27 / L mu_r = 0.1 incl = math.radians(98) m_eu98 = np.array([[0., 0., -1.], [math.cos(incl), math.sin(incl), 0.], [math.sin(incl), -math.cos(incl), 0.]]) #m_eu0 = np.array([[0.,0.,-1.],[1.,0.,0.],[0,-1.,0.]]) q0 = qnv.rotm2quat(m_eu98) q0 = q0.reshape((4, 1)) pos0 = np.array([[R + 6e3], [0.], [0.]]) dist0 = np.linalg.norm(pos0) #v0 = np.array([[0.],[math.sqrt(G*M/dist0)],[0.]]) v0 = np.array([[0], [math.sqrt(G * M / dist0) * math.cos(math.radians(98))], [math.sqrt(G * M / dist0) * math.sin(math.radians(98))]]) w0 = np.array([[0.], [-1 * math.sqrt(G * M / dist0**3)], [0.]]) #w0 = np.array([[0.], [0.], [0.]]) state0 = np.vstack((pos0, v0, q0, w0))