def test_quat_inverse():
vec = np.array([.33, .66, .99])
q = np.array([sqrt(.25), sqrt(.25), sqrt(.25), sqrt(.25)])
vec_rotated = ecpp.rotate_vec(vec, q)
q_inv = ecpp.get_inverse_quaternion(q)
vec_rot_back = ecpp.rotate_vec(vec_rotated, q_inv)
np.testing.assert_allclose(vec, vec_rot_back, atol=1e-15)
def test_quat_rot1():
theta = pi / 4
q = np.array([cos(theta / 2), 0, 0, sin(theta / 2)
]) # 45 degree rotation about the z axis body to inertial
xb = np.array([1, 0, 0])
zb = np.array([0, 0, 1])
x_sol = np.array([cos(theta), sin(theta), 0])
z_sol = np.array([0, 0, 1])
np.testing.assert_allclose(ecpp.rotate_vec(xb, q), x_sol, atol=1e-15)
np.testing.assert_allclose(ecpp.rotate_vec(zb, q), z_sol, atol=1e-15)
def test_quat_rot6():
theta = pi / 4
q = np.array([cos(theta / 2), 0, 0, sin(theta / 2)
]) # 45 degree rotation about the z axis body to inertial
yb = np.array([0, 1, 0])
y_sol = np.array([-sin(theta), cos(theta), 0])
np.testing.assert_allclose(ecpp.rotate_vec(yb, q), y_sol, atol=1e-15)
def test_quat_rot4():
theta = pi / 3
q = np.array([cos(theta / 2), sin(theta / 2), 0,
0]) # 60 degree rotation about the x axis body to inertial
xb = np.array([0, 1, 0])
x_sol = np.array([0, cos(theta), sin(theta)])
np.testing.assert_allclose(ecpp.rotate_vec(xb, q), x_sol, atol=1e-15)
[-B_field_NED[2]]
]) # north, east, down to east, north, up
# get magnetic field in body frame (for detumble algorithm)
# R_body2ECI = quat2DCM(np.transpose(x[0:4]))
B_field_ECEF = np.transpose(R_ECEF2ENU) @ B_field_ENU
B_field_ECI = np.transpose(R_ECI2ECEF) @ B_field_ECEF
# Correct to max expected value of Earth's magnetic field if pyIGRF throws a huge value
# if i > 0:
# if np.linalg.norm(B_field_ECI) > 7e-08:
# B_field_ECI = B_field_ECI/np.linalg.norm(B_field_ECI)*np.linalg.norm(B_field_ECI_vec[i-1,:])#* 7e-08
B_field_ECI_vec[i, :] = np.transpose(B_field_ECI)
q_ECI2body = ecpp.get_inverse_quaternion(x[0:4])
B_field_body[i, :] = ecpp.rotate_vec(B_field_ECI, q_ECI2body)
# Get B_dot based on previous measurement
if i > 0:
B_1 = np.transpose(B_field_body[i - 1, :])
B_2 = np.transpose(B_field_body[i, :])
B_dot = dcpp.get_B_dot(B_1, B_2, tstep)
B_dot_body[i, :] = np.transpose(B_dot)
# Validate B_dot algorithm
# k_B_dot = -5e-6
# dipole = dcpp.detumble_B_dot(np.transpose(B_field_body[i,:]),B_dot, k_B_dot)
# Torque on spacecraft
# M = np.cross(dipole, np.transpose(B_field_body[i, :]))
# Validate B_dot bang bang control law: