def lla2enu(init_lla,point_lla): n_EA_E = nv.lat_lon2n_E(init_lla[0], init_lla[1]) n_EB_E = nv.lat_lon2n_E(point_lla[0], point_lla[1]) p_AB_E = nv.n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, init_lla[2], point_lla[2]) R_EN = nv.n_E2R_EN(n_EA_E) p_AB_N = np.dot(R_EN.T, p_AB_E).ravel() p_AB_N[0],p_AB_N[1] = p_AB_N[1],p_AB_N[0] return p_AB_N
def test_n_E_and_wa2R_EL(): n_E = np.array([[0], [0], [1]]) R_EL = n_E_and_wa2R_EL(n_E, wander_azimuth=np.pi / 2) R_EL1 = [[0, 1.0, 0], [1.0, 0, 0], [0, 0, -1.0]] assert_array_almost_equal(R_EL, R_EL1) R_EN = n_E2R_EN(n_E) assert_array_almost_equal(R_EN, np.diag([-1, 1, -1])) n_E1 = R_EL2n_E(R_EN) n_E2 = R_EN2n_E(R_EN) assert_array_almost_equal(n_E, n_E1) assert_array_almost_equal(n_E, n_E2)
def test_n_E_and_wa2R_EL(): n_E = np.array([[0], [0], [1]]) R_EL = n_E_and_wa2R_EL(n_E, wander_azimuth=np.pi/2) R_EL1 = [[0, 1.0, 0], [1.0, 0, 0], [0, 0, -1.0]] assert_array_almost_equal(R_EL, R_EL1) R_EN = n_E2R_EN(n_E) assert_array_almost_equal(R_EN, np.diag([-1, 1, -1])) n_E1 = R_EL2n_E(R_EN) n_E2 = R_EN2n_E(R_EN) assert_array_almost_equal(n_E, n_E1) assert_array_almost_equal(n_E, n_E2)
def test_Ex1_A_and_B_to_delta_in_frame_N(): # Positions A and B are given in (decimal) degrees and depths: lat_EA, lon_EA, z_EA = rad(1), rad(2), 3 lat_EB, lon_EB, z_EB = rad(4), rad(5), 6 # Find the exact vector between the two positions, given in meters # north, east, and down, i.e. find p_AB_N. # SOLUTION: # Step1: Convert to n-vectors (rad() converts to radians): n_EA_E = lat_lon2n_E(lat_EA, lon_EA) n_EB_E = lat_lon2n_E(lat_EB, lon_EB) # Step2: Find p_AB_E (delta decomposed in E). # WGS-84 ellipsoid is default: p_AB_E = n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB) # Step3: Find R_EN for position A: R_EN = n_E2R_EN(n_EA_E) # Step4: Find p_AB_N p_AB_N = np.dot(R_EN.T, p_AB_E) # (Note the transpose of R_EN: The "closest-rule" says that when # decomposing, the frame in the subscript of the rotation matrix that # is closest to the vector, should equal the frame where the vector is # decomposed. Thus the calculation np.dot(R_NE, p_AB_E) is correct, # since the vector is decomposed in E, and E is closest to the vector. # In the example we only had R_EN, and thus we must transpose it: # R_EN'=R_NE) # Step5: Also find the direction (azimuth) to B, relative to north: azimuth = np.arctan2(p_AB_N[1], p_AB_N[0]) # positive angle about down-axis print('Ex1, delta north, east, down = {0}, {1}, {2}'.format(p_AB_N[0], p_AB_N[1], p_AB_N[2])) print('Ex1, azimuth = {0} deg'.format(deg(azimuth))) assert_array_almost_equal(p_AB_N[0], 331730.23478089) assert_array_almost_equal(p_AB_N[1], 332997.87498927) assert_array_almost_equal(p_AB_N[2], 17404.27136194) assert_array_almost_equal(deg(azimuth), 45.10926324)
def test_Ex1_A_and_B_to_delta_in_frame_N(): # Positions A and B are given in (decimal) degrees and depths: lat_EA, lon_EA, z_EA = rad(1), rad(2), 3 lat_EB, lon_EB, z_EB = rad(4), rad(5), 6 # Find the exact vector between the two positions, given in meters # north, east, and down, i.e. find p_AB_N. # SOLUTION: # Step1: Convert to n-vectors (rad() converts to radians): n_EA_E = lat_lon2n_E(lat_EA, lon_EA) n_EB_E = lat_lon2n_E(lat_EB, lon_EB) # Step2: Find p_AB_E (delta decomposed in E). # WGS-84 ellipsoid is default: p_AB_E = n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB) # Step3: Find R_EN for position A: R_EN = n_E2R_EN(n_EA_E) # Step4: Find p_AB_N p_AB_N = np.dot(R_EN.T, p_AB_E) # (Note the transpose of R_EN: The "closest-rule" says that when # decomposing, the frame in the subscript of the rotation matrix that # is closest to the vector, should equal the frame where the vector is # decomposed. Thus the calculation np.dot(R_NE, p_AB_E) is correct, # since the vector is decomposed in E, and E is closest to the vector. # In the example we only had R_EN, and thus we must transpose it: # R_EN'=R_NE) # Step5: Also find the direction (azimuth) to B, relative to north: azimuth = np.arctan2(p_AB_N[1], p_AB_N[0]) # positive angle about down-axis print('Ex1, delta north, east, down = {0}, {1}, {2}'.format( p_AB_N[0], p_AB_N[1], p_AB_N[2])) print('Ex1, azimuth = {0} deg'.format(deg(azimuth))) assert_array_almost_equal(p_AB_N[0], 331730.23478089) assert_array_almost_equal(p_AB_N[1], 332997.87498927) assert_array_almost_equal(p_AB_N[2], 17404.27136194) assert_array_almost_equal(deg(azimuth), 45.10926324)
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)
def setNed(self, current, target): lat_C, lon_C = rad(current.lat), rad(current.lon) lat_T, lon_T = rad(target.lat), rad(target.lon) # create an n-vector for current and target nvecC = nv.lat_lon2n_E(lat_C, lon_C) nvecT = nv.lat_lon2n_E(lat_T, lon_T) # create a p-vec from C to T in the Earth's frame # the zeros are for the depth (depth = -1 * altitude) p_CT_E = nv.n_EA_E_and_n_EB_E2p_AB_E(nvecC, nvecT, 0, 0) # create a rotation matrix # this rotates points from the NED frame to the Earth's frame R_EN = nv.n_E2R_EN(nvecC) # rotate p_CT_E so it lines up with current's NED frame # we use the transpose so we can go from the Earth's frame to the NED frame n, e, d = np.dot(R_EN.T, p_CT_E).ravel() #Scale Nedvalues if n > e: scaleFactor = abs(1.00000 / n) else: scaleFactor = abs(1.00000 / e) return Nedvalues(n * scaleFactor, e * scaleFactor, d)
def n_E2R_EN(self): n_E = self.to_nvector().get_xyz(shape=(3, 1)) R_EN = nv.n_E2R_EN(n_E) return R_EN
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]