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)