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))
