Beispiel #1
0
    def test_Ex8_position_A_and_azimuth_and_distance_to_B():

        # Position A is given as n_EA_E:
        # Enter elements as lat/long in deg:
        lat, lon = rad(80), rad(-90)

        n_EA_E = lat_lon2n_E(lat, lon)

        # The initial azimuth and great circle distance (s_AB), and Earth
        # radius (r_Earth) are also given:
        azimuth = rad(200)
        s_AB = 1000  # m
        r_Earth = 6371e3  # m, mean Earth radius

        # Find the destination point B, as n_EB_E ("The direct/first geodetic
        # problem" for a sphere)

        # SOLUTION:
        # Step1: Convert distance in meter into distance in [rad]:
        distance_rad = s_AB / r_Earth
        # Step2: Find n_EB_E:
        n_EB_E = n_EA_E_distance_and_azimuth2n_EB_E(n_EA_E, distance_rad,
                                                    azimuth)

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EB, long_EB = n_E2lat_lon(n_EB_E)
        print('Ex8, Destination: lat, long = {0} {1} deg'.format(
            deg(lat_EB), deg(long_EB)))

        assert_array_almost_equal(deg(lat_EB), 79.99154867)
        assert_array_almost_equal(deg(long_EB), -90.01769837)
        azimuth1 = n_EA_E_and_n_EB_E2azimuth(n_EA_E, n_EB_E, a=r_Earth, f=0)
        assert_array_almost_equal(azimuth, azimuth1 + 2 * np.pi)
Beispiel #2
0
    def test_Ex9_intersect():

        # Two paths A and B are given by two pairs of positions:
        # Enter elements as lat/long in deg:
        n_EA1_E = lat_lon2n_E(rad(10), rad(20))
        n_EA2_E = lat_lon2n_E(rad(30), rad(40))
        n_EB1_E = lat_lon2n_E(rad(50), rad(60))
        n_EB2_E = lat_lon2n_E(rad(70), rad(80))

        # Find the intersection between the two paths, n_EC_E:
        n_EC_E_tmp = unit(
            np.cross(np.cross(n_EA1_E, n_EA2_E, axis=0),
                     np.cross(n_EB1_E, n_EB2_E, axis=0),
                     axis=0))

        # n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select
        # the one that is closet to n_EA1_E, by selecting sign from the dot
        # product between n_EC_E_tmp and n_EA1_E:
        n_EC_E = np.sign(np.dot(n_EC_E_tmp.T, n_EA1_E)) * n_EC_E_tmp

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EC, long_EC = n_E2lat_lon(n_EC_E)
        msg = 'Ex9, Intersection: lat, long = {} {} deg'
        print(msg.format(deg(lat_EC), deg(long_EC)))
        assert_array_almost_equal(deg(lat_EC), 40.31864307)
        assert_array_almost_equal(deg(long_EC), 55.90186788)
Beispiel #3
0
    def test_Ex6_interpolated_position():

        # Position B at time t0 and t2 is given as n_EB_E_t0 and n_EB_E_t1:
        # Enter elements as lat/long in deg:
        n_EB_E_t0 = lat_lon2n_E(rad(89), rad(0))
        n_EB_E_t1 = lat_lon2n_E(rad(89), rad(180))

        # The times are given as:
        t0 = 10
        t1 = 20
        ti = 16  # time of interpolation

        # Find the interpolated position at time ti, n_EB_E_ti

        # SOLUTION:
        # Using standard interpolation:
        n_EB_E_ti = unit(n_EB_E_t0 + (ti - t0) * (n_EB_E_t1 - n_EB_E_t0) /
                         (t1 - t0))

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EB_ti, long_EB_ti = n_E2lat_lon(n_EB_E_ti)
        msg = 'Ex6, Interpolated position: lat, long = {} {} deg'
        print(msg.format(deg(lat_EB_ti), deg(long_EB_ti)))

        assert_array_almost_equal(deg(lat_EB_ti), 89.7999805)
        assert_array_almost_equal(deg(long_EB_ti), 180.)
Beispiel #4
0
def plot_mean_position():
    """
    Example
    -------
    >>> plot_mean_position()
    Ex7, Average lat=[ 67.23615295], lon=[-6.91751117]
    """
    positions = np.array([
        (90, 0),
        (60, 10),
        (50, -20),
    ])
    lats, lons = positions.T
    nvecs = lat_lon2n_E(rad(lats), rad(lons))

    # Find the horizontal mean position:
    n_EM_E = unit(np.sum(nvecs, axis=1).reshape((3, 1)))
    lat, lon = n_E2lat_lon(n_EM_E)
    lat, lon = deg(lat), deg(lon)
    print(('Ex7, Average lat={0}, lon={1}'.format(lat, lon)))

    map1 = Basemap(projection='ortho',
                   lat_0=int(lat),
                   lon_0=int(lon),
                   resolution='l')
    plot_world(map1)
    x, y = map1(lon, lat)
    map1.scatter(x, y, linewidth=5, marker='o', color='r')

    x1, y1 = map1(lons, lats)
    # print(len(lons), x1, y1)
    map1.scatter(x1, y1, linewidth=5, marker='o', color='k')

    plt.title('Figure of mean position (red dot) compared to positions '
              'A, B, and C (black dots).')
Beispiel #5
0
def plot_mean_position():
    positions = np.array([(90, 0),
                          (60, 10),
                          (50, -20),
                          ])
    lats, lons = positions.T
    nvecs = lat_lon2n_E(rad(lats), rad(lons))

    # Find the horizontal mean position:
    n_EM_E = unit(np.sum(nvecs, axis=1).reshape((3, 1)))
    lat, lon = n_E2lat_lon(n_EM_E)
    lat, lon = deg(lat), deg(lon)
    print('Ex7, Average lat={0}, lon={1}'.format(lat, lon))

    map1 = Basemap(projection='ortho', lat_0=int(lat), lon_0=int(lon),
                   resolution='l')
    plot_world(map1)
    x, y = map1(lon, lat)
    map1.scatter(x, y, linewidth=5, marker='o', color='r')

    x1, y1 = map1(lons, lats)
    print(len(lons), x1, y1)
    map1.scatter(x1, y1, linewidth=5, marker='o', color='k')

    plt.title('Figure of mean position (red dot) compared to positions '
              'A, B, and C (black dots).')
Beispiel #6
0
def plot_mean_position():
    """
    Example
    -------
    >>> plot_mean_position()
    Ex7, Average lat=67.2, lon=-6.9
    >>> plt.show()  # doctest: +SKIP
    >>> plt.close()
    """
    positions = np.array([
        (90, 0),
        (60, 10),
        (50, -20),
    ])
    lats, lons = np.transpose(positions)
    nvecs = lat_lon2n_E(rad(lats), rad(lons))

    # Find the horizontal mean position:
    n_EM_E = unit(np.sum(nvecs, axis=1).reshape((3, 1)))
    lat, lon = n_E2lat_lon(n_EM_E)
    lat, lon = deg(lat), deg(lon)
    print('Ex7, Average lat={0:2.1f}, lon={1:2.1f}'.format(lat[0], lon[0]))

    plotter = _init_plotter(lat, lon)

    plotter(lon, lat, linewidth=5, marker='o', color='r')
    plotter(lons, lats, linewidth=5, marker='o', color='k')

    plt.title('Figure of mean position (red dot) compared to \npositions '
              'A, B, and C (black dots).')
Beispiel #7
0
def find_c_point_from_precalc(to_point, point1, point2, c12, p1h, p2h, dp1p2):
    tpn = to_point.nv
    ctp = cross(tpn, c12, axis=0)
    try:
        c = unit(cross(ctp, c12, axis=0))
    except Exception:
        print((to_point, point1, point2))
        raise
    sutable_c = None
    for co in (c, 0 - c):
        co_rs = co.reshape((3, ))
        dp1co = arccos(arccos_limit(dot(p1h, co_rs)))
        dp2co = arccos(arccos_limit(dot(p2h, co_rs)))
        if abs(dp1co + dp2co - dp1p2) < 0.000001:
            sutable_c = co
            break

    if sutable_c is not None:
        c_point_lat, c_point_lng = n_E2lat_lon(sutable_c)
        c_point = Point(lat=rad2deg(c_point_lat[0]),
                        lng=rad2deg(c_point_lng[0]))
        c_dist = distance(to_point, c_point)
    else:
        c_dist, c_point = min(
            ((distance(to_point, p), p) for p in (point1, point2)),
            key=itemgetter(0))

    return find_c_point_result(c_dist, c_point)
Beispiel #8
0
    def test_Ex9_intersection():

        # Two paths A and B are given by two pairs of positions:
        # Enter elements as lat/long in deg:
        n_EA1_E = lat_lon2n_E(rad(10), rad(20))
        n_EA2_E = lat_lon2n_E(rad(30), rad(40))
        n_EB1_E = lat_lon2n_E(rad(50), rad(60))
        n_EB2_E = lat_lon2n_E(rad(70), rad(80))

        # Find the intersection between the two paths, n_EC_E:
        n_EC_E_tmp = unit(np.cross(np.cross(n_EA1_E, n_EA2_E, axis=0),
                                   np.cross(n_EB1_E, n_EB2_E, axis=0), axis=0))

        # n_EC_E_tmp is one of two solutions, the other is -n_EC_E_tmp. Select
        # the one that is closet to n_EA1_E, by selecting sign from the dot
        # product between n_EC_E_tmp and n_EA1_E:
        n_EC_E = np.sign(np.dot(n_EC_E_tmp.T, n_EA1_E)) * n_EC_E_tmp

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EC, long_EC = n_E2lat_lon(n_EC_E)
        msg = 'Ex9, Intersection: lat, long = {} {} deg'
        print(msg.format(deg(lat_EC), deg(long_EC)))
        assert_array_almost_equal(deg(lat_EC), 40.31864307)
        assert_array_almost_equal(deg(long_EC), 55.90186788)
Beispiel #9
0
    def test_Ex6_interpolated_position():

        # Position B at time t0 and t2 is given as n_EB_E_t0 and n_EB_E_t1:
        # Enter elements as lat/long in deg:
        n_EB_E_t0 = lat_lon2n_E(rad(89), rad(0))
        n_EB_E_t1 = lat_lon2n_E(rad(89), rad(180))

        # The times are given as:
        t0 = 10
        t1 = 20
        ti = 16  # time of interpolation

        # Find the interpolated position at time ti, n_EB_E_ti

        # SOLUTION:
        # Using standard interpolation:
        n_EB_E_ti = unit(n_EB_E_t0 +
                         (ti - t0) * (n_EB_E_t1 - n_EB_E_t0) / (t1 - t0))

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EB_ti, long_EB_ti = n_E2lat_lon(n_EB_E_ti)
        msg = 'Ex6, Interpolated position: lat, long = {} {} deg'
        print(msg.format(deg(lat_EB_ti), deg(long_EB_ti)))

        assert_array_almost_equal(deg(lat_EB_ti), 89.7999805)
        assert_array_almost_equal(deg(long_EB_ti), 180.)
Beispiel #10
0
    def test_Ex8_position_A_and_azimuth_and_distance_to_B():

        # Position A is given as n_EA_E:
        # Enter elements as lat/long in deg:
        lat, lon = rad(80), rad(-90)

        n_EA_E = lat_lon2n_E(lat, lon)

        # The initial azimuth and great circle distance (s_AB), and Earth
        # radius (r_Earth) are also given:
        azimuth = rad(200)
        s_AB = 1000  # m
        r_Earth = 6371e3  # m, mean Earth radius

        # Find the destination point B, as n_EB_E ("The direct/first geodetic
        # problem" for a sphere)

        # SOLUTION:
        # Step1: Convert distance in meter into distance in [rad]:
        distance_rad = s_AB / r_Earth
        # Step2: Find n_EB_E:
        n_EB_E = n_EA_E_distance_and_azimuth2n_EB_E(n_EA_E, distance_rad,
                                                    azimuth)

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EB, long_EB = n_E2lat_lon(n_EB_E)
        print('Ex8, Destination: lat, long = {0} {1} deg'.format(deg(lat_EB),
                                                               deg(long_EB)))

        assert_array_almost_equal(deg(lat_EB), 79.99154867)
        assert_array_almost_equal(deg(long_EB), -90.01769837)
        azimuth1 = n_EA_E_and_n_EB_E2azimuth(n_EA_E, n_EB_E, a=r_Earth, f=0)
        assert_array_almost_equal(azimuth, azimuth1+2*np.pi)
Beispiel #11
0
def find_closest_point_pair(points,
                            to_point,
                            req_min_dist=20,
                            stop_after_dist=50):
    tpn = to_point.nv
    min_distance = None
    min_point_pair = None
    min_c_point = None
    for point1, point2 in pairs(points):
        p1 = point1.nv
        p2 = point2.nv
        c12 = cross(p1, p2, axis=0)
        ctp = cross(tpn, c12, axis=0)
        c = unit(cross(ctp, c12, axis=0))
        p1h = p1.reshape((3, ))
        p2h = p2.reshape((3, ))
        dp1p2 = arccos(dot(p1h, p2h))

        sutable_c = None
        for co in (c, 0 - c):
            co_rs = co.reshape((3, ))
            dp1co = arccos(dot(p1h, co_rs))
            dp2co = arccos(dot(p2h, co_rs))
            if abs(dp1co + dp2co - dp1p2) < 0.000001:
                sutable_c = co
                break

        if sutable_c is not None:
            c_point_lat, c_point_lng = n_E2lat_lon(sutable_c)
            c_point = Point(lat=rad2deg(c_point_lat[0]),
                            lng=rad2deg(c_point_lng[0]))
            c_dist = distance(to_point, c_point)
        else:
            c_dist, c_point = min(
                ((distance(to_point, p), p) for p in (point1, point2)))

        if min_distance is None or c_dist < min_distance:
            min_distance = c_dist
            min_point_pair = (point1, point2)
            min_c_point = c_point

        if min_distance < req_min_dist and c_dist > stop_after_dist:
            break

    return min_point_pair, min_c_point, min_distance
Beispiel #12
0
    def test_Ex3_ECEF_vector_to_geodetic_latitude():

        # Position B is given as p_EB_E ("ECEF-vector")

        p_EB_E = 6371e3 * np.vstack((0.9, -1, 1.1))  # m

        # Find position B as geodetic latitude, longitude and height

        # SOLUTION:
        # Find n-vector from the p-vector:
        n_EB_E, z_EB = p_EB_E2n_EB_E(p_EB_E)

        # Convert to lat, long and height:
        lat_EB, long_EB = n_E2lat_lon(n_EB_E)
        h_EB = -z_EB
        msg = 'Ex3, Pos B: lat, long = {} {} deg, height = {} m'
        print(msg.format(deg(lat_EB), deg(long_EB), h_EB))

        assert_array_almost_equal(deg(lat_EB), 39.37874867)
        assert_array_almost_equal(deg(long_EB), -48.0127875)
        assert_array_almost_equal(h_EB, 4702059.83429485)
Beispiel #13
0
    def test_Ex3_ECEF_vector_to_geodetic_latitude():

        # Position B is given as p_EB_E ("ECEF-vector")

        p_EB_E = 6371e3 * np.vstack((0.9, -1, 1.1))  # m

        # Find position B as geodetic latitude, longitude and height

        # SOLUTION:
        # Find n-vector from the p-vector:
        n_EB_E, z_EB = p_EB_E2n_EB_E(p_EB_E)

        # Convert to lat, long and height:
        lat_EB, long_EB = n_E2lat_lon(n_EB_E)
        h_EB = -z_EB
        msg = 'Ex3, Pos B: lat, long = {} {} deg, height = {} m'
        print(msg.format(deg(lat_EB), deg(long_EB), h_EB))

        assert_array_almost_equal(deg(lat_EB), 39.37874867)
        assert_array_almost_equal(deg(long_EB), -48.0127875)
        assert_array_almost_equal(h_EB, 4702059.83429485)
Beispiel #14
0
    def test_Ex2_B_and_delta_in_frame_B_to_C_in_frame_E():
        # delta vector from B to C, decomposed in B is given:
        p_BC_B = np.r_[3000, 2000, 100].reshape((-1, 1))

        # Position and orientation of B is given:
        n_EB_E = unit([[1], [2], [3]])  # unit to get unit length of vector
        z_EB = -400
        R_NB = zyx2R(rad(10), rad(20), rad(30))
        # the three angles are yaw, pitch, and roll

        # A custom reference ellipsoid is given (replacing WGS-84):
        a, f = 6378135, 1.0 / 298.26  # (WGS-72)

        # Find the position of C.
        # SOLUTION:
        # Step1: Find R_EN:
        R_EN = n_E2R_EN(n_EB_E)

        # Step2: Find R_EB, from R_EN and R_NB:
        R_EB = np.dot(R_EN, R_NB)  # Note: closest frames cancel

        # Step3: Decompose the delta vector in E:
        p_BC_E = np.dot(R_EB, p_BC_B)
        # no transpose of R_EB, since the vector is in B

        # Step4: Find the position of C, using the functions that goes from one
        # position and a delta, to a new position:
        n_EC_E, z_EC = n_EA_E_and_p_AB_E2n_EB_E(n_EB_E, p_BC_E, z_EB, a, f)

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EC, long_EC = n_E2lat_lon(n_EC_E)
        # Here we also assume that the user wants output height (= - depth):
        msg = 'Ex2, Pos C: lat, long = {},{} deg,  height = {} m'
        print(msg.format(deg(lat_EC), deg(long_EC), -z_EC))

        assert_array_almost_equal(deg(lat_EC), 53.32637826)
        assert_array_almost_equal(deg(long_EC), 63.46812344)
        assert_array_almost_equal(z_EC, -406.00719607)
Beispiel #15
0
    def test_Ex2_B_and_delta_in_frame_B_to_C_in_frame_E():
        # delta vector from B to C, decomposed in B is given:
        p_BC_B = np.r_[3000, 2000, 100].reshape((-1, 1))

        # Position and orientation of B is given:
        n_EB_E = unit([[1], [2], [3]])  # unit to get unit length of vector
        z_EB = -400
        R_NB = zyx2R(rad(10), rad(20), rad(30))
        # the three angles are yaw, pitch, and roll

        # A custom reference ellipsoid is given (replacing WGS-84):
        a, f = 6378135, 1.0 / 298.26  # (WGS-72)

        # Find the position of C.
        # SOLUTION:
        # Step1: Find R_EN:
        R_EN = n_E2R_EN(n_EB_E)

        # Step2: Find R_EB, from R_EN and R_NB:
        R_EB = np.dot(R_EN, R_NB)  # Note: closest frames cancel

        # Step3: Decompose the delta vector in E:
        p_BC_E = np.dot(R_EB, p_BC_B)
        # no transpose of R_EB, since the vector is in B

        # Step4: Find the position of C, using the functions that goes from one
        # position and a delta, to a new position:
        n_EC_E, z_EC = n_EA_E_and_p_AB_E2n_EB_E(n_EB_E, p_BC_E, z_EB, a, f)

        # When displaying the resulting position for humans, it is more
        # convenient to see lat, long:
        lat_EC, long_EC = n_E2lat_lon(n_EC_E)
        # Here we also assume that the user wants output height (= - depth):
        msg = 'Ex2, Pos C: lat, long = {},{} deg,  height = {} m'
        print(msg.format(deg(lat_EC), deg(long_EC), -z_EC))

        assert_array_almost_equal(deg(lat_EC), 53.32637826)
        assert_array_almost_equal(deg(long_EC), 63.46812344)
        assert_array_almost_equal(z_EC, -406.00719607)
Beispiel #16
0
 def from_nv(cls, nv, round_digits=6):
     lat, lng = n_E2lat_lon(nv)
     point = cls(round(rad2deg(lat[0]), round_digits),
                 round(rad2deg(lng[0]), round_digits))
     point._nv = nv
     return point
Beispiel #17
0
    def to_lla(self):
        lat, lon = nv.n_E2lat_lon(self.n_EB_E)

        return Lla(np.rad2deg(lat[0]), np.rad2deg(lon[0]), -self.depth)
Beispiel #18
0
def pvec_b_to_lla(forward, right, down, roll, pitch, yaw, lat, lon, alt):
    """
    returns the lat lon and alt corresponding to a p-vec in the UAV's body frame

    Parameters
    ----------
    forward: float
        The number of meters forward of the UAV
    right: float
        The number of meters to the right of the UAV
    down: float
        The number of meters below the UAV
    roll: float
        The UAV's roll angle in degrees
    pitch: float
        The UAV's pitch angle in degrees
    yaw: float
        The UAV's yaw angle in degrees
    lat: float
        The UAV's latitude in degrees
    lon: float
        The UAV's longitude in degrees
    alt: float
        The UAV's altitude in meters

    Returns
    -------
    list
        This list holds three floats representing the latitude in degrees, longitude in degrees and altitude in meters (in that order).
    """

    # create a p-vector with the forward, right and down values
    p_B = np.array([forward, right, down])

    # this matrix can transform a pvec in the body frame to a pvec in the NED frame
    rot_NB = nv.zyx2R(radians(yaw), radians(pitch), radians(roll))

    # calculate the pvec in the NED frame
    p_N = rot_NB.dot(p_B)

    # create an n-vector for the UAV
    n_UAV = nv.lat_lon2n_E(radians(lat), radians(lon))

    # this creates a matrix that rotates pvecs from NED to ECEF
    rot_EN = nv.n_E2R_EN(n_UAV)

    # find the offset vector from the UAV to the point of interest in the ECEF frame
    p_delta_E = rot_EN.dot(p_N)

    # find the p-vector for the UAV in the ECEF frame
    p_EUAV_E = nv.n_EB_E2p_EB_E(n_UAV, -alt).reshape(1, 3)[0]

    # find the p-vector for the point of interest. This is the UAV + the offset in the ECEF frame.
    p_E = p_EUAV_E + p_delta_E

    # find the n-vector for the point of interest given the p-vector in the ECEF frame.
    n_result, z_result = nv.p_EB_E2n_EB_E(p_E.reshape(3, 1))

    # convert the n-vector to a lat and lon
    lat_result, lon_result = nv.n_E2lat_lon(n_result)
    lat_result, lon_result = degrees(lat_result), degrees(lon_result)

    # convert depth to alt
    alt_result = -z_result[0]

    return [lat_result, lon_result, alt_result]
Beispiel #19
0
def getMiddlePointFromList(positions):
    n_EM_E = nv.unit(sum(positions))
    lat, lon = nv.n_E2lat_lon(n_EM_E)
    lat, lon = degrees(lat[0]), degrees(lon[0])

    return lat, lon