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 test_stationary_body(self,value): v_w_IO_o = np.array([0.,0.00106178,0.]) qBO = np.asarray(value) w_BOB = qnv.quatRotate(qBO,v_w_IO_o) result = fr.wBOb2wBIb(w_BOB,qBO,v_w_IO_o) expected = np.array([0.,0.,0.]) self.assertTrue(np.allclose(result,expected))
def test_ideal_controlled(self): v_w_IO_o = np.array([0.,0.00106178,0.]) qBO = np.array([1.,0.,0.,0.]) w_BOB = np.array([0.,0.,0.]) w_BIB = fr.wBOb2wBIb(w_BOB,qBO,v_w_IO_o) expected = -qnv.quatRotate(qBO,v_w_IO_o) self.assertTrue(np.allclose(w_BIB,expected))
def getW_BI_b(
self
): #get exact angular velocity of body frame with respect to inertial frame expressed in body frame
v_w_BO_b = self.v_state[4:7]
v_q_BO = self.v_state[0:4]
v_w_BI_b = fs.wBOb2wBIb(
v_w_BO_b, v_q_BO, v_w_IO_o
) #calculation of wBIb from wBOb stored in frames using wBOb2wBIb (function of frames) (v_w_IO_o from constants_1U)
return v_w_BI_b
def test_dynamics(self):
t0 = 0.
h = 0.001
v_q_BO = np.array([0.4, 0.254, -0.508, 0.71931912])
v_q_BI = frames.qBO2qBI(v_q_BO, np.array([1e6, 53e5, 0.]),
np.array([5.60, -5.0, 0.0]))
mySat = satellite.Satellite(
np.hstack((v_q_BI, np.array([0.1, -0.05, -0.3]))), t0)
mySat.setPos(np.array([1e6, 53e5, 0.]))
mySat.setVel(np.array([5.60, -5.0, 0.0]))
mySat.setDisturbance_b(np.array([10e-10, -4e-6, -3e-5]))
mySat.setControl_b(np.array([1e-5, 1e-5, -8e-4]))
r = np.linalg.norm(mySat.getPos())
v_w_IO_o = np.array([
0., np.sqrt(G * M_EARTH / (r)**3), 0.
]) #angular velocity of orbit frame wrt inertial frame in orbit frame
v_w_BO_b = frames.wBIb2wBOb(np.array([0.1, -0.05, -0.3]), v_q_BO,
v_w_IO_o)
x1 = rk4Quaternion(mySat, x_dot_BO, h)
mySat.setState(x1.copy())
mySat.setTime(t0 + h)
k1 = h * x_dot_BO(mySat, t0 + 0.5 * h, np.hstack((v_q_BO, v_w_BO_b)))
k2 = h * x_dot_BO(mySat, t0 + 0.5 * h,
np.hstack((v_q_BO, v_w_BO_b)) + 0.5 * k1)
k3 = h * x_dot_BO(mySat, t0 + 0.5 * h,
np.hstack((v_q_BO, v_w_BO_b)) + 0.5 * k2)
k4 = h * x_dot_BO(mySat, t0 + h, np.hstack((v_q_BO, v_w_BO_b)) + k3)
error_state = np.hstack(
(v_q_BO, v_w_BO_b)) + (1. / 6.) * (k1 + 2. * k2 + 2. * k3 + k4)
expected = np.hstack((frames.qBO2qBI(error_state[0:4],
np.array([1e6, 53e5, 0.]),
np.array([5.60, -5.0, 0.0])),
frames.wBOb2wBIb(error_state[4:7],
error_state[0:4], v_w_IO_o)))
self.assertTrue(np.allclose(expected, mySat.getState()))
else:
print ("setVel or getVel incorrect")
Sat4.setQ_BO(q1)
Q=Sat4.getQ_BO()
if (Q==q1).all():
print ("setQ_BO and getQ_BO correct")
else:
print ("setQ_BO and getQ_BO incorrect")
Sat4.setW_BO_b(w)
a=Sat4.getW_BO_b()
b = Sat4.getW_BI_b()
w2 = fs.wBOb2wBIb(w,q1,v_w_IO_o)
if (a==w).all():
print ("setW_BO_b and getW_BO_b correct")
else:
print ("setW_BO_b and getW_BO_b incorrect")
if (b==w2).all():
print ("getW_BI_b correct")
else:
print ("getW_BI_b incorrect")
expected = np.array([-0.5*np.sqrt(1+0.5*np.sqrt(3.)),
-0.25*np.sqrt(2/(2+np.sqrt(3))), 0.25*np.sqrt(2/(2+np.sqrt(3))), 0.5*np.sqrt(1+0.5*np.sqrt(3.))])
Sat4.setPos(np.array([0.,0.,7.07e6]))
Sat4.setVel(np.array([0.,7.0e3,0.]))
Sat4.setQ_BO((0.5/np.sqrt(2.))*np.array([np.sqrt(3.),1.,1.,np.sqrt(3.)]))