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_quat2rotm(self, value):
q = np.asarray(value[0])
m1 = np.asarray(value[1])
m2 = np.asarray(value[2])
m3 = np.asarray(value[3])
A = qnv.quat2rotm(q)
self.assertTrue(np.allclose(A[0, :], m1))
self.assertTrue(np.allclose(A[1, :], m2))
self.assertTrue(np.allclose(A[2, :], m3))
def calc_v_mag_b_e(v_mag_b_m, v_mag_o, q_k1k):
quatk1km = qnv.quat2rotm(q_k1k)
v_mag_b_e = np.dot(quatk1km, v_mag_o)
return v_mag_b_e
def w_bob_calc(w_m_k, q_kk, w_oio, b_e):
R = qnv.quat2rotm(q_kk)
w_bib = w_m_k - b_e #estimated omega
w_bob = w_bib - np.dot(R, w_oio)
return w_bob
import qnv
import satellite as sat
I3 = np.matrix('1,0,0;0,1,0;0,0,1') #defining 3x3 identity matrix
I4 = np.matrix(
'1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1') #defining 4x4 identity matrix
I6 = np.matrix(
'1,0,0,0,0,0;0,1,0,0,0,0;0,0,1,0,0,0;0,0,0,1,0,0;0,0,0,0,1,0;0,0,0,0,0,1')
P_k = np.matrix(
'1,0,0,0,0,0;0,1,0,0,0,0;0,0,1,0,0,0;0,0,0,1,0,0;0,0,0,0,1,0;0,0,0,0,0,1'
) #initial state covariance matrix
q_kk = np.matrix('0,0,0,1').T
b = np.matrix('1,1,1').T
b_e = np.matrix('0,0,0').T
w_m_k = np.matrix('1,1,1').T
w_oio = np.matrix('1,1,1').T
quat = qnv.quat2rotm(np.squeeze(np.asarray(q_kk)))
q = np.asmatrix(quat)
delta_b = b - b_e #delta b, diffence between gyro bias and estimated bias
w_bib = w_m_k - b_e #estimated omega
w_bob = w_bib - (q * w_oio)
delta_theta = q_kk[0:3, 0] / q_kk[
3,
0] #vector part of error quaternion normalised such that scalar part is equated to 1
delta_x = np.array([
delta_theta[0], delta_theta[1], delta_theta[2], delta_b[0], delta_b[1],
delta_b[2]
]) #error state vector
A = I3 - qnv.skew(w_bob)
B = -I3
t = 1