Transform local North-East-Down to parent (e.g. GEO) coordinates

ned_coords.py

import numpy as np


# Set up transformation matrices
def calc_transmat(lat, lon):
    rlon = np.deg2rad(lon)
    rlat = np.deg2rad(lat)

    transmat = dict()
    #now transform from local tangent plane coordinates to parent
    transmat['enu_ecef'] = np.zeros((3,3))
    # eastward unit vector in parent
    transmat['enu_ecef'][0, 0] = -np.sin(rlon)
    transmat['enu_ecef'][1, 0] = np.cos(rlon)
    # northward unit vector in parent
    transmat['enu_ecef'][0, 1] = -np.cos(rlon)*np.sin(rlat)
    transmat['enu_ecef'][1, 1] = -np.sin(rlat)*np.sin(rlon)
    transmat['enu_ecef'][2, 1] = np.cos(rlat)
    # radial outward unit vector in parent
    transmat['enu_ecef'][0, 2] = np.cos(rlat)*np.cos(rlon)
    transmat['enu_ecef'][1, 2] = np.sin(rlon)*np.cos(rlat)
    transmat['enu_ecef'][2, 2] = np.sin(rlat)

    transmat['ecef_enu'] = transmat['enu_ecef'].T
    return transmat


def ned_to_parent(invec, lat, lon):
    """one input vector at a time...

    invec is vector in local NED system
    lat, lon are in parent system

    outpos is vector in parent system
    """
    #turn ned into enu for convenience
    enu = np.asarray(invec)[[1, 0, 2]] * np.array([-1, 1, -1])
    trmat = calc_transmat(lat, lon)['enu_ecef']
    outvec = trmat.dot(enu)
    return outvec


def parent_to_ned(invec, lat, lon):
    """ one at a time

    invec is cartesian parent vector
    lat, lon are in parent system
    outpos is NED in local NED system
    """
    trmat = calc_transmat(lat, lon)['ecef_enu']
    enu = trmat.dot(invec)
    ned = np.asarray(enu)[[1, 0, 2]] * np.array([1, -1, -1])
    return ned