def get_heading_bci(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot):
rbcf = x_bcf[0:3]
vbcf = x_bcf[3:6]
m_u = math_utilities.math_utilities_class()
xi1, d_long_dx = get_longitude(x_bcf, t, theta_1, theta_2, theta_3,
ellipsmat, w0, wdot) # aux. angle xi1 (rad)
# transformation matrix from BCF frame to F1 frame
R_bcf2f1 = m_u.transform3(xi1)
n_pa = get_n_pa(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot)
R_bcf2pa = get_R_bcf2pa(theta_1, theta_2, theta_3)
R_pa2bcf = np.transpose(R_bcf2pa)
n_f1 = np.dot(R_bcf2f1,
np.dot(R_pa2bcf,
n_pa)) # normal vector expressed in F1 frame
xi2 = np.asscalar(m_u.atan2(n_f1[0], n_f1[2])) # aux. angle xi2 (rad)
# transformation matrix from F1 frame to F2 frame
R_f12f2 = m_u.transform2(xi2)
vbci_in_f2 = np.dot(R_f12f2, np.dot(
R_bcf2f1,
vbci_in_bcf)) # velocity w.r.t. BCI frame EXPRESSED in F2 frame (km/s)
# heading angle using velocity with respect to the BCI frame (rad)
heading_bci = np.asscalar(m_u.atan2(vbci_in_f2[1], vbci_in_f2[0]))
return heading_bci
def __init__(self,
a=1.0,
b=1.0,
c=1.0,
t=0.0,
r_pa=[1.0, 0.0, 0.0],
vbcf=[0.0, 0.0, 0.0],
theta1=0.0,
theta2=0.0,
theta3=0.0,
w0=0.0,
w0dot=0.0):
"""Constructor"""
# Note that we take in r_pa rather than r_bcf so that we can be sure that the position is actually on the ellipse
self.m_u = math_utilities.math_utilities_class()
self.a = a
self.b = b
self.c = c
self.ex = (a**2 - c**2) / a**2
self.ee = (a**2 - b**2) / a**2
self.calc_ellipsmat()
# BCF frame unit vectors, expressed in BCF frame
self.i_bcf = self.m_u.i_unit()
self.j_bcf = self.m_u.j_unit()
self.k_bcf = self.m_u.k_unit()
self.set_state(t, r_pa, vbcf, theta1, theta2, theta3, w0, w0dot)
return
def get_r_pa(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot):
rbcf = x_bcf[0:3]
vbcf = x_bcf[3:6]
m_u = math_utilities.math_utilities_class()
R_bcf2pa = get_R_bcf2pa(theta_1, theta_2, theta_3)
r_pa = np.dot(R_bcf2pa, rbcf)
return r_pa
def get_R_bcf2pa(theta_1, theta_2, theta_3):
m_u = math_utilities.math_utilities_class()
R_bcf2fprime = m_u.transform3(theta_1)
R_fprime2fdprime = m_u.transform1(theta_2)
R_fdprime2pa = m_u.transform3(theta_3)
R_bcf2pa = np.dot(R_fdprime2pa, np.dot(R_fprime2fdprime, R_bcf2fprime))
return R_bcf2pa
def get_vbci_in_bcf(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot):
rbcf = x_bcf[0:3]
vbcf = x_bcf[3:6]
m_u = math_utilities.math_utilities_class()
omega = get_omega(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot)
# velocity vector w.r.t. BCI frame (km/s). vector is still EXPRESSED in the BCF frame
vbci_in_bcf = vbcf + np.cross(omega, rbcf, axis=0)
# derivatives
d_vbci_in_bcf_dx = np.zeros((3, 6), dtype=np.complex128)
d_vbci_in_bcf_dx[0:3, 0:3] = m_u.crossmat(omega) # d/drbcf
d_vbci_in_bcf_dx[0:3, 3:6] = np.diag([1.0, 1.0, 1.0]) # d/dvbcf
return vbci_in_bcf, d_vbci_in_bcf_dx
def get_longitude(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot):
rbcf = x_bcf[0:3]
vbcf = x_bcf[3:6]
m_u = math_utilities.math_utilities_class()
# BCF longitude (rad)
longitude = m_u.atan2(np.asscalar(rbcf[1, 0]), np.asscalar(rbcf[0, 0]))
# derivatives
d_atan2 = m_u.d_atan2(rbcf[1, 0], rbcf[0, 0])
d_long_drbcf = np.zeros((1, 3), dtype=np.complex128)
d_long_drbcf[0, 0:3] = np.array(
[np.asscalar(d_atan2[1]),
np.asscalar(d_atan2[0]), 0.0])
d_long_dvbcf = np.zeros((1, 3))
d_long_dvbcf[0, 0:3] = np.array([0.0, 0.0, 0.0])
d_long_dt = np.array([0.0])
d_long_dx = np.zeros((1, 6))
d_long_dx = np.concatenate((d_long_drbcf, d_long_dvbcf), axis=1)
#d_long_dx = np.concatenate((d_long_dx, d_long_dt), axis=0)
return longitude, d_long_dx
def get_latitude(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot):
rbcf = x_bcf[0:3]
vbcf = x_bcf[3:6]
m_u = math_utilities.math_utilities_class()
longitude, d_long_dx = get_longitude(x_bcf, t, theta_1, theta_2, theta_3,
ellipsmat, w0, wdot)
# BCF bodycentric latitude (rad)
phi = m_u.atan2(
rbcf[2, 0],
(rbcf[0, 0] * np.cos(longitude) + rbcf[1, 0] * np.sin(longitude)))
# derivatives
d_phi_d_rbcf = np.zeros((1, 3), dtype=np.complex128)
d_phi_d_arg = m_u.d_atan2(
rbcf[2, 0],
rbcf[0, 0] * np.cos(longitude) + rbcf[1, 0] * np.sin(longitude))
d_argx_d_rbcf = np.zeros((1, 3), dtype=np.complex128)
d_coslong_d_rbcf = np.array([
-np.sin(longitude) * d_long_dx[0, 0],
-np.sin(longitude) * d_long_dx[0, 1], 0.0
])
d_sinlong_d_rbcf = np.array([
np.cos(longitude) * d_long_dx[0, 0],
np.cos(longitude) * d_long_dx[0, 1], 0.0
])
d_argx_d_rbcf[0, 0] = np.asscalar(
np.cos(longitude) + rbcf[0, 0] * d_coslong_d_rbcf[0] +
rbcf[1, 0] * d_sinlong_d_rbcf[0])
d_argx_d_rbcf[0, 1] = rbcf[0, 0] * d_coslong_d_rbcf[1] + np.sin(
longitude) + rbcf[1, 0] * d_sinlong_d_rbcf[1]
d_argx_d_rbcf[0, 2] = 0.0
d_argy_d_rbcf = np.array([0.0, 0.0, 1.0], dtype=np.complex128)
d_phi_d_rbcf = np.asscalar(d_phi_d_arg[1]) * d_argx_d_rbcf + np.asscalar(
d_phi_d_arg[0]) * d_argy_d_rbcf
d_phi_d_vbcf = np.zeros((1, 3), dtype=np.complex128)
d_phi_dt = np.array([0.0], dtype=np.complex128)
d_phi_dx = np.concatenate((d_phi_d_rbcf, d_phi_d_vbcf), axis=1)
#d_phi_dx = np.concatenate((d_phi_dx, d_phi_dt), axis=1)
return phi, d_phi_dx
def get_bcf2seu_mat_rotations(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat,
w0, wdot):
"""calculate the transformation matrix from BCF frame to SEU frame using rotations to define orthogonal vectors"""
rbcf = x_bcf[0:3]
vbcf = x_bcf[3:6]
m_u = math_utilities.math_utilities_class()
# ellipsoid normal vector in PA frame
n_pa = get_n_pa(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot)
n_pa_mag = (
n_pa[0]**2 + n_pa[1]**2 + n_pa[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
n_pa_unit = n_pa / n_pa_mag # unit vector
# transformation matrix between BCF and PA frames
R_bcf2pa = get_R_bcf2pa(theta_1, theta_2, theta_3)
R_pa2bcf = np.transpose(R_bcf2pa)
# u (up) vector in BCF frame, i.e., n_bcf
u_bcf = np.dot(R_pa2bcf, n_pa)
u_bcf_mag = (
u_bcf[0]**2 + u_bcf[1]**2 + u_bcf[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
u_bcf_unit = u_bcf / u_bcf_mag # unit vector
# get longitude (= xi1)
longitude, d_long_dx = get_longitude(x_bcf, t, theta_1, theta_2, theta_3,
ellipsmat, w0, wdot)
# transformation matrix from BCF frame to F1 frame
R_bcf2f1 = m_u.transform3(longitude)
# up/normal vector in F1 frame
u_f1 = np.dot(R_bcf2f1, u_bcf)
# angle xi2 between F1 and F2 frames
xi2 = m_u.atan2(np.asscalar(u_f1[0]), np.asscalar(u_f1[2]))
# transformation matrix from F1 frame to F2 frame
R_f12f2 = m_u.transform2(xi2)
# up/normal vector in F2 frame
u_f2 = np.dot(R_f12f2, u_f1)
# angle xi3 between F2 and F3 frames
xi3 = m_u.atan2(np.asscalar(u_f2[1]), np.asscalar(u_f2[2]))
# transformation matrix from F2 frame to F3 frame
R_f22f3 = m_u.transform1(xi3)
# combine rotation matrices
R_bcf2seu = np.dot(R_f22f3, np.dot(R_f12f2, R_bcf2f1))
# unit vectors of SEU in BCF coordinates just for testing
# s is i unit vector in F3
s_bcf_unit = np.dot(R_bcf2seu, m_u.i_unit())
# e is j unit vector in F3
e_bcf_unit = np.dot(R_bcf2seu, m_u.j_unit())
#print("Unit vectors from rotation matrix method")
#print('s: ', s_bcf_unit)
#print('e: ', e_bcf_unit)
#print('u: ', u_bcf_unit)
#print('')
return R_bcf2seu
def get_bcf2seu_mat_crossp(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0,
wdot):
"""calculate the transformation matrix from BCF frame to SEU frame using cross products to define orthogonal vectors"""
rbcf = x_bcf[0:3]
vbcf = x_bcf[3:6]
m_u = math_utilities.math_utilities_class()
# BCF frame unit vectors, expressed in BCF frame
i_bcf = m_u.i_unit()
j_bcf = m_u.j_unit()
k_bcf = m_u.k_unit()
# ellipsoid normal vector in PA frame
n_pa = get_n_pa(x_bcf, t, theta_1, theta_2, theta_3, ellipsmat, w0, wdot)
n_pa_mag = (
n_pa[0]**2 + n_pa[1]**2 + n_pa[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
n_pa_unit = n_pa / n_pa_mag # unit vector
# transformation matrix between BCF and PA frames
R_bcf2pa = get_R_bcf2pa(theta_1, theta_2, theta_3)
R_pa2bcf = np.transpose(R_bcf2pa)
# u (up) vector in BCF frame, i.e., n_bcf
u_bcf = np.dot(R_pa2bcf, n_pa)
u_bcf_mag = (
u_bcf[0]**2 + u_bcf[1]**2 + u_bcf[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
u_bcf_unit = u_bcf / u_bcf_mag # unit vector
# e (east) vector in BCF frame
firstarg = m_u.crossmat(k_bcf)
secondarg = 2.0 * np.dot(R_pa2bcf, np.dot(ellipsmat, np.dot(
R_bcf2pa, rbcf)))
e_bcf = np.dot(firstarg, secondarg)
e_bcf_mag = (
e_bcf[0]**2 + e_bcf[1]**2 + e_bcf[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
e_bcf_unit = e_bcf / e_bcf_mag # unit vector
# s (south) vector in BCF frame
e_bcf_cross = m_u.crossmat(e_bcf)
s_bcf = np.dot(e_bcf_cross, u_bcf)
s_bcf_mag = (
s_bcf[0]**2 + s_bcf[1]**2 + s_bcf[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
s_bcf_unit = s_bcf / s_bcf_mag # unit vect
# s (south) vector in BCF frame that actually points toward the pole; not necessarily perpendicular to east as defined in this routine
firstarg = m_u.crossmat(k_bcf)
secondarg = rbcf
eps_bcf = np.dot(firstarg, secondarg)
eps_bcf_mag = (
eps_bcf[0]**2 + eps_bcf[1]**2 + eps_bcf[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
eps_bcf_unit = eps_bcf / eps_bcf_mag # unit vector
eps_bcf_cross = m_u.crossmat(eps_bcf)
s_pole_bcf = np.dot(eps_bcf_cross, u_bcf)
s_pole_bcf_mag = (
s_pole_bcf[0]**2 + s_pole_bcf[1]**2 + s_pole_bcf[2]**2
)**0.5 # using this instead of norm() for the benefit of CX derivatives
s_pole_bcf_unit = s_pole_bcf / s_pole_bcf_mag # unit vect
# transformation matrix
R_bcf2seu = np.zeros((3, 3))
R_bcf2seu[0, 0] = np.dot(np.transpose(s_bcf_unit), i_bcf)
R_bcf2seu[0, 1] = np.dot(np.transpose(s_bcf_unit), j_bcf)
R_bcf2seu[0, 2] = np.dot(np.transpose(s_bcf_unit), k_bcf)
R_bcf2seu[1, 0] = np.dot(np.transpose(e_bcf_unit), i_bcf)
R_bcf2seu[1, 1] = np.dot(np.transpose(e_bcf_unit), j_bcf)
R_bcf2seu[1, 2] = np.dot(np.transpose(e_bcf_unit), k_bcf)
R_bcf2seu[2, 0] = np.dot(np.transpose(u_bcf_unit), i_bcf)
R_bcf2seu[2, 1] = np.dot(np.transpose(u_bcf_unit), j_bcf)
R_bcf2seu[2, 2] = np.dot(np.transpose(u_bcf_unit), k_bcf)
print("Unit vectors from cross product method")
print('s: ', s_bcf_unit)
print('s_pole: ', s_pole_bcf_unit)
print('e: ', e_bcf_unit)
print('eps: ', eps_bcf_unit)
print('u: ', u_bcf_unit)
print('')
return R_bcf2seu, e_bcf_unit, s_bcf_unit, s_pole_bcf_unit
s_bcf_unit = np.dot(R_bcf2seu, m_u.i_unit())
# e is j unit vector in F3
e_bcf_unit = np.dot(R_bcf2seu, m_u.j_unit())
#print("Unit vectors from rotation matrix method")
#print('s: ', s_bcf_unit)
#print('e: ', e_bcf_unit)
#print('u: ', u_bcf_unit)
#print('')
return R_bcf2seu
# class instantiation
m_u = math_utilities.math_utilities_class()
# Parameters defining ellipsoid (km)
#a = 6378.14 # semimajor axis (Earth)
#b = 6378.14 # semiminaor axis (Earth)
#c = 6356.75 # semiintermediate axis (Earth)
a = 1.7
b = 1.1
c = 0.8
ellipsmat = get_ellipsmat(a, b, c) # get matrix of ellipsoid parameters
# Euler angles between BCF frame and PA frame in rad (angles assumed to be constant)
#theta_1 = np.deg2rad(40.)
#theta_2 = np.deg2rad(70.)
#theta_3 = np.deg2rad(10.)