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