def x_dot_BO(sat): #need m_INERTIA (abot center of mass)
'''
This function calculates the derivative of quaternion (q_BO)
and angular velocity w_BOB
Input: satellite, time
Output: Differential state vector
'''
#get torques acting about COM
v_torque_control_b = sat.getControl_b() #Control torque
v_torque_dist_b = sat.getDisturbance_b() #Disturbance torque
v_torque_b = v_torque_control_b + v_torque_dist_b
#get current state
v_q_BO = sat.getQ_BO() #unit quaternion rotating from orbit to body
v_w_BO_b = sat.getW_BO_b() #angular velocity of body frame wrt orbit frame in body frame
R = quat2rotm(v_q_BO)
#Kinematic equation
#print(v_q_BO)
#print(v_w_BO_b)
v_q_BO_dot = quatDerBO(v_q_BO,v_w_BO_b)
v_w_BI_b = frames.wBOb2wBIb(v_w_BO_b,v_q_BO,v_w_IO_o)
v_w_OI_o = -v_w_IO_o.copy()
#Dynamic equation - Euler equation of motion
v_w_BO_b_dot = np.dot(m_INERTIA_inv,v_torque_b - np.cross(v_w_BI_b,np.dot(m_INERTIA,v_w_BI_b))) - np.cross ( np.dot(R, v_w_OI_o), v_w_BO_b)
v_x_dot = np.hstack((v_q_BO_dot,v_w_BO_b_dot))
#print(v_x_dot)
return v_x_dot
def propogate_quaternion(w_bob, q_kk):
#state at t = t0
#rk-4 routine (updating satellite class state with obtained state at every step of rk4 routine)
#first step of rk4 routine
dq = t * qnv.quatDerBO(q_kk, w_bob)
q_k1k = q_kk + dq
return q_k1k
def test_quatDerBO(self, value):
w1 = np.asarray(value[1])
q = np.asarray(value[0])
qdot = np.asarray(value[2])
A = qnv.quatDerBO(q, w1)
self.assertTrue(np.allclose(A, qdot))
