Ejemplo n.º 1
0
    def test_R2zxy():
        x, y, z = rad((10, 20, 30))
        R_AB1 = zyx2R(z, y, x)
        R_AB = [[0.813798, -0.44097, 0.378522], [0.469846, 0.882564, 0.018028],
                [-0.34202, 0.163176, 0.925417]]

        assert_array_almost_equal(R_AB, R_AB1)
        z1, y1, x1 = R2zyx(R_AB1)
        assert_array_almost_equal((x, y, z), (x1, y1, z1))
Ejemplo n.º 2
0
    def test_R2zxy():
        x, y, z = rad((10, 20, 30))
        R_AB1 = zyx2R(z, y, x)
        R_AB = [[0.813798, -0.44097, 0.378522],
                [0.469846, 0.882564, 0.018028],
                [-0.34202, 0.163176, 0.925417]]

        assert_array_almost_equal(R_AB, R_AB1)
        z1, y1, x1 = R2zyx(R_AB1)
        assert_array_almost_equal((x, y, z), (x1, y1, z1))
Ejemplo n.º 3
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)
Ejemplo n.º 4
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)
Ejemplo n.º 5
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]