def test_zero_torque_ideal_q(self): #zero torque, random initial angular velocity, aligned frames qBO = np.array([.0,0.,0.,1.]) vel = np.array([5.60,-5.0,0.0]) state = np.hstack((qBO,np.array([-0.1,0.39159385,-0.7]))) mySat = satellite.Satellite(state,12.05) mySat.setVel(vel) mySat.setDisturbance_b(np.array([0.,0.,0.])) mySat.setControl_b(np.array([0.,0.,0.])) result = x_dot_BO(mySat) self.assertTrue(np.allclose(result,[-0.05,0.1957969257,-0.35, 0., 0.00453874, -0.00185081, -0.00168015]))
def test_kinematics_explicitly(self): qBO = np.array([0.254,-0.508,np.sqrt(1-0.4**2-0.254**2-0.508**2),0.4]) vel = np.array([5.60,-5.0,0.0]) state = np.hstack((qBO,np.array([0.10050588,-0.05026119,-0.3014887]))) mySat = satellite.Satellite(state,128.05) mySat.setVel(vel) mySat.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat.setControl_b(np.array([1e-5,1e-5,-8e-4])) result = x_dot_BO(mySat) self.assertTrue(np.allclose(result,[0.11475622, 0.06438473, -0.04115242, 0.08290271, 0.00632537, 0.00313621, -0.56264705]))
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()))
def test_dynamics(self,value): t0 = 0. h = value v_q_BO = np.array([0.4,0.254,-0.508,0.71931912]) v_w_BO_b = frames.wBIb2wBOb(np.array([0.1,-0.05,-0.3]),v_q_BO,v_w_IO_o) mySat1 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat1.setPos(np.array([1e6,53e5,0.])) mySat1.setVel(np.array([5.60,-5.0,0.0])) mySat1.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat1.setControl_b(np.array([1e-5,1e-5,-8e-4])) updateStateTimeRK4(mySat1,x_dot_BO,h) x1 = mySat1.getState() mySat2 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat2.setPos(np.array([1e6,53e5,0.])) mySat2.setVel(np.array([5.60,-5.0,0.0])) mySat2.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat2.setControl_b(np.array([1e-5,1e-5,-8e-4])) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))) k1 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*k1) k2 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*(k2)) k3 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+k3) k4 = h*x_dot_BO(mySat2) error_state = np.hstack((v_q_BO,v_w_BO_b)) + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4) if (error_state[3]<0): error_state[0:4]=-1*error_state[0:4] error_state[0:4]=error_state[0:4]/np.linalg.norm(error_state[0:4]) self.assertTrue(np.allclose(error_state,x1))
def test_dynamics(self): t0 = 0. h = 0.001 v_q_BO = np.array([0.4,0.254,-0.508,0.71931912]) v_w_BO_b = frames.wBIb2wBOb(np.array([0.1,-0.05,-0.3]),v_q_BO,v_w_IO_o) mySat1 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat1.setPos(np.array([1e6,53e5,0.])) mySat1.setVel(np.array([5.60,-5.0,0.0])) mySat1.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat1.setControl_b(np.array([1e-5,1e-5,-8e-4])) rk4Quaternion(mySat1,x_dot_BO,h) x1 = mySat1.getState() mySat2 = satellite.Satellite(np.hstack((v_q_BO, v_w_BO_b)),t0) mySat2.setPos(np.array([1e6,53e5,0.])) mySat2.setVel(np.array([5.60,-5.0,0.0])) mySat2.setDisturbance_b(np.array([10e-10,-4e-6,-3e-5])) mySat2.setControl_b(np.array([1e-5,1e-5,-8e-4])) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))) k1 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*k1) k2 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+0.5*(k2+k1)) k3 = h*x_dot_BO(mySat2) mySat2.setState(np.hstack((v_q_BO,v_w_BO_b))+k3+0.5*(k2+k1)) k4 = h*x_dot_BO(mySat2) error_state = np.hstack((v_q_BO,v_w_BO_b)) + (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4) print(error_state) self.assertTrue(np.allclose(error_state,x1))
def test_zero_torque_ideal_q(
self
): #zero torque, random initial angular velocity, aligned frames
qBO = np.array([1.0, 0., 0., 0.])
pos = np.array([1e6, 53e5, 0.])
vel = np.array([5.60, -5.0, 0.0])
state = np.hstack((qBO, np.array([-0.1, 0.39159385, -0.7])))
mySat = satellite.Satellite(state, 12.05)
mySat.setPos(pos)
mySat.setVel(vel)
mySat.setDisturbance_b(np.array([0., 0., 0.]))
mySat.setControl_b(np.array([0., 0., 0.]))
result = x_dot_BO(mySat, 12.06, mySat.getState())
self.assertTrue(
np.allclose(result, [
0., -0.05, 0.1957969257, -0.35, 0.00490396, -0.00185206,
-0.00173161
]))
def test_kinematics_explicitly(self):
qBO = np.array(
[0.4, 0.254, -0.508,
np.sqrt(1 - 0.4**2 - 0.254**2 - 0.508**2)])
pos = np.array([1e6, 53e5, 0.])
vel = np.array([5.60, -5.0, 0.0])
state = np.hstack((qBO, np.array([0.10050588, -0.05026119,
-0.3014887])))
mySat = satellite.Satellite(state, 128.05)
mySat.setPos(pos)
mySat.setVel(vel)
mySat.setDisturbance_b(np.array([10e-10, -4e-6, -3e-5]))
mySat.setControl_b(np.array([1e-5, 1e-5, -8e-4]))
result = x_dot_BO(mySat, 128.08, mySat.getState())
print(result)
self.assertTrue(
np.allclose(result, [
0.08290271, 0.11475622, 0.06438473, -0.04115242, 0.00627683,
0.00299226, -0.56264081
]))
def test_zero_torque_rates_ideal_q(self): #zero torque, zero initial wBIB, aligned frames ''' For this test case set (from sixth model of Advitiy) and ALTITUDE = 700 km Ixx = 0.00152529 Iyy = 0.00145111 Izz = 0.001476 Ixy = 0.00000437 Iyz = - 0.00000408 Ixz = 0.00000118 ''' qBO = np.array([0.0,0.,0.0,1.]) vel = np.array([5.0,5.0,0.0]) v_w_BI_b = np.array([0.,0.,0.]) v_w_BO_b = frames.wBIb2wBOb(v_w_BI_b,qBO,v_w_IO_o) state = np.hstack((qBO,v_w_BO_b)) mySat = satellite.Satellite(state,12.0) mySat.setVel(vel) mySat.setDisturbance_b(np.array([0.,0.,0.])) mySat.setControl_b(np.array([0.,0.,0.])) result = x_dot_BO(mySat) self.assertTrue(np.allclose(result,[0.,0.00053089,0.,0.,0.,0.,0.]))
def test_zero_torque_rates_ideal_q(
self): #zero torque, zero initial wBIB, aligned frames
'''
For this test case set (from sixth model of Advitiy)
Ixx = 0.00152529
Iyy = 0.00145111
Izz = 0.001476
Ixy = 0.00000437
Iyz = - 0.00000408
Ixz = 0.00000118
'''
qBO = np.array([1.0, 0., 0., 0.])
pos = np.array([1e6, 0., 0.])
vel = np.array([5.0, 5.0, 0.0])
state = np.hstack((qBO, np.array([0., 0.01996437, 0.])))
mySat = satellite.Satellite(state, 12.0)
mySat.setPos(pos)
mySat.setVel(vel)
mySat.setDisturbance_b(np.array([0., 0., 0.]))
mySat.setControl_b(np.array([0., 0., 0.]))
result = x_dot_BO(mySat, 12.0, mySat.getState())
self.assertTrue(
np.allclose(result, [0., 0., 0.009982185, 0., 0., 0., 0.]))
