def ecef_euler_from_ned(ned_ecef_init, ned_pose): ''' Got it from here: Using Rotations to Build Aerospace Coordinate Systems -Don Koks ''' converter = LocalCoord.from_ecef(ned_ecef_init) x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0]) y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0]) z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0]) x1 = rot(z0, ned_pose[2]).dot(x0) y1 = rot(z0, ned_pose[2]).dot(y0) z1 = rot(z0, ned_pose[2]).dot(z0) x2 = rot(y1, ned_pose[1]).dot(x1) y2 = rot(y1, ned_pose[1]).dot(y1) z2 = rot(y1, ned_pose[1]).dot(z1) x3 = rot(x2, ned_pose[0]).dot(x2) y3 = rot(x2, ned_pose[0]).dot(y2) #z3 = rot(x2, ned_pose[0]).dot(z2) x0 = array([1, 0, 0]) y0 = array([0, 1, 0]) z0 = array([0, 0, 1]) psi = np.arctan2(inner(x3, y0), inner(x3, x0)) theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2)) y2 = rot(z0, psi).dot(y0) z2 = rot(y2, theta).dot(z0) phi = np.arctan2(inner(y3, z2), inner(y3, y2)) ret = array([phi, theta, psi]) return ret
def ned_euler_from_ecef(ned_ecef_init, ecef_poses):
'''
Got the math from here:
Using Rotations to Build Aerospace Coordinate Systems
-Don Koks
Also accepts array of ecef_poses and array of ned_ecef_inits.
Where each row is a pose and an ecef_init.
'''
ned_ecef_init = array(ned_ecef_init)
ecef_poses = array(ecef_poses)
output_shape = ecef_poses.shape
ned_ecef_init = np.atleast_2d(ned_ecef_init)
if ned_ecef_init.shape[0] == 1:
ned_ecef_init = np.tile(ned_ecef_init[0], (output_shape[0], 1))
ecef_poses = np.atleast_2d(ecef_poses)
ned_poses = np.zeros(ecef_poses.shape)
for i, ecef_pose in enumerate(ecef_poses):
converter = LocalCoord.from_ecef(ned_ecef_init[i])
x0 = array([1, 0, 0])
y0 = array([0, 1, 0])
z0 = array([0, 0, 1])
x1 = rot(z0, ecef_pose[2]).dot(x0)
y1 = rot(z0, ecef_pose[2]).dot(y0)
z1 = rot(z0, ecef_pose[2]).dot(z0)
x2 = rot(y1, ecef_pose[1]).dot(x1)
y2 = rot(y1, ecef_pose[1]).dot(y1)
z2 = rot(y1, ecef_pose[1]).dot(z1)
x3 = rot(x2, ecef_pose[0]).dot(x2)
y3 = rot(x2, ecef_pose[0]).dot(y2)
#z3 = rot(x2, ecef_pose[0]).dot(z2)
x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
psi = np.arctan2(inner(x3, y0), inner(x3, x0))
theta = np.arctan2(-inner(x3, z0),
np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
y2 = rot(z0, psi).dot(y0)
z2 = rot(y2, theta).dot(z0)
phi = np.arctan2(inner(y3, z2), inner(y3, y2))
ned_poses[i] = array([phi, theta, psi])
return ned_poses.reshape(output_shape)