def test_Ex10_cross_track_distance(): # Position A1 and A2 and B are given as n_EA1_E, n_EA2_E, and n_EB_E: # Enter elements as lat/long in deg: n_EA1_E = lat_lon2n_E(rad(0), rad(0)) n_EA2_E = lat_lon2n_E(rad(10), rad(0)) n_EB_E = lat_lon2n_E(rad(1), rad(0.1)) radius = 6371e3 # m, mean Earth radius # Find the cross track distance from path A to position B. # SOLUTION: # Find the unit normal to the great circle: c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0)) # Find the great circle cross track distance: s_xt = -np.arcsin(np.dot(c_E.T, n_EB_E)) * radius # Find the Euclidean cross track distance: d_xt = -np.dot(c_E.T, n_EB_E) * radius msg = 'Ex10, Cross track distance = {} m, Euclidean = {} m' print(msg.format(s_xt, d_xt)) assert_array_almost_equal(s_xt, 11117.79911015) assert_array_almost_equal(d_xt, 11117.79346741)
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)
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)
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.)
def test_Ex10_cross_track_distance(): # Position A1 and A2 and B are given as n_EA1_E, n_EA2_E, and n_EB_E: # Enter elements as lat/long in deg: n_EA1_E = lat_lon2n_E(rad(0), rad(0)) n_EA2_E = lat_lon2n_E(rad(10), rad(0)) n_EB_E = lat_lon2n_E(rad(1), rad(0.1)) r_Earth = 6371e3 # m, mean Earth radius # Find the cross track distance from path A to position B. # SOLUTION: # Find the unit normal to the great circle: c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0)) # Find the great circle cross track distance: s_xt = (np.arccos(np.dot(c_E.T, n_EB_E)) - np.pi / 2) * r_Earth # Find the Euclidean cross track distance: d_xt = -np.dot(c_E.T, n_EB_E) * r_Earth msg = 'Ex10, Cross track distance = {} m, Euclidean = {} m' print(msg.format(s_xt, d_xt)) assert_array_almost_equal(s_xt, 11117.79911015) assert_array_almost_equal(d_xt, 11117.79346741)
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_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)
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).')
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).')
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).')
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)
def to_nvector(self): lat = self.get_latitude(as_rad=True) lon = self.get_longitude(as_rad=True) alt = self.get_altitude() n_EB_E = nv.lat_lon2n_E(lat, lon) x, y, z = n_EB_E.ravel() return Nvector(x, y, z, -alt)
def test_Ex5_great_circle_distance(): # Position A and B are given as n_EA_E and n_EB_E: # Enter elements as lat/long in deg: n_EA_E = lat_lon2n_E(rad(88), rad(0)) n_EB_E = lat_lon2n_E(rad(89), rad(-170)) r_Earth = 6371e3 # m, mean Earth radius # SOLUTION: s_AB = great_circle_distance(n_EA_E, n_EB_E, radius=r_Earth) d_AB = euclidean_distance(n_EA_E, n_EB_E, radius=r_Earth) msg = 'Ex5, Great circle distance = {} km, Euclidean distance = {} km' print(msg.format(s_AB / 1000, d_AB / 1000)) assert_array_almost_equal(s_AB / 1000, 332.45644411) assert_array_almost_equal(d_AB / 1000, 332.41872486)
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_Ex7_mean_position(): # Three positions A, B and C are given: # Enter elements as lat/long in deg: n_EA_E = lat_lon2n_E(rad(90), rad(0)) n_EB_E = lat_lon2n_E(rad(60), rad(10)) n_EC_E = lat_lon2n_E(rad(50), rad(-20)) # Find the horizontal mean position: n_EM_E = unit(n_EA_E + n_EB_E + n_EC_E) # The result is best viewed with a figure that shows the n-vectors # relative to an Earth-model: print('Ex7, See figure') # plot_Earth_figure(n_EA_E,n_EB_E,n_EC_E,n_EM_E) assert_array_almost_equal(n_EM_E.ravel(), [0.384117, -0.046602, 0.922107]) # Alternatively: n_EM_E = mean_horizontal_position(np.hstack((n_EA_E, n_EB_E, n_EC_E))) assert_array_almost_equal(n_EM_E.ravel(), [0.384117, -0.046602, 0.922107])
def test_small_and_large_cross_track_distance(self): radius = 6371e3 # m, mean Earth radius n_EA1_E = lat_lon2n_E(rad(5), rad(10)) n_EA2_E = lat_lon2n_E(rad(10), rad(10)) n_EB0_E = lat_lon2n_E(rad(7), rad(10.1)) path = (n_EA1_E, n_EA2_E) n_EB1_E = closest_point_on_great_circle(path, n_EB0_E) for s_xt0 in [np.pi / 3 * radius, 10., 0.1, 1e-4, 1e-8]: distance_rad = s_xt0 / radius n_EB_E = n_EA_E_distance_and_azimuth2n_EB_E( n_EB1_E, distance_rad, np.pi / 2) n_EB2_E = closest_point_on_great_circle(path, n_EB_E) s_xt = great_circle_distance(n_EB1_E, n_EB_E, radius) c_E = unit(np.cross(n_EA1_E, n_EA2_E, axis=0)) s_xt2 = (np.arccos(np.dot(c_E.T, n_EB_E)) - np.pi / 2) * radius s_xt3 = cross_track_distance(path, n_EB_E, method='greatcircle', radius=radius) s_xt4 = np.arctan2( -np.dot(c_E.T, n_EB_E), np.linalg.norm(np.cross(c_E, n_EB_E, axis=0), axis=0)) * radius assert_array_almost_equal(n_EB2_E, n_EB1_E) assert_array_almost_equal(s_xt, s_xt0) assert_array_almost_equal(s_xt2, s_xt0) assert_array_almost_equal(s_xt3, s_xt0) assert_array_almost_equal(s_xt4, s_xt0) rtol = 10**(-min(9 + np.log10(s_xt0), 15)) self.assertTrue(np.abs(s_xt - s_xt0) / s_xt0 < rtol, 's_xt fails') self.assertTrue( np.abs(s_xt2 - s_xt0) / s_xt0 < rtol, 's_xt2 fails') self.assertTrue( np.abs(s_xt3 - s_xt0) / s_xt0 < rtol, 's_xt3 fails') self.assertTrue( np.abs(s_xt4 - s_xt0) / s_xt0 < rtol, 's_xt4 fails')
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 test_Ex4_geodetic_latitude_to_ECEF_vector(): # Position B is given with lat, long and height: lat_EB_deg = 1 long_EB_deg = 2 h_EB = 3 # Find the vector p_EB_E ("ECEF-vector") # SOLUTION: # Step1: Convert to n-vector: n_EB_E = lat_lon2n_E(rad(lat_EB_deg), rad(long_EB_deg)) # Step2: Find the ECEF-vector p_EB_E: p_EB_E = n_EB_E2p_EB_E(n_EB_E, -h_EB) print('Ex4: p_EB_E = {0} m'.format(p_EB_E.ravel())) assert_array_almost_equal(p_EB_E.ravel(), [6373290.27721828, 222560.20067474, 110568.82718179])
def test_Ex4_geodetic_latitude_to_ECEF_vector(): # Position B is given with lat, long and height: lat_EB_deg = 1 long_EB_deg = 2 h_EB = 3 # Find the vector p_EB_E ("ECEF-vector") # SOLUTION: # Step1: Convert to n-vector: n_EB_E = lat_lon2n_E(rad(lat_EB_deg), rad(long_EB_deg)) # Step2: Find the ECEF-vector p_EB_E: p_EB_E = n_EB_E2p_EB_E(n_EB_E, -h_EB) print('Ex4: p_EB_E = {0} m'.format(p_EB_E.ravel())) assert_array_almost_equal( p_EB_E.ravel(), [6373290.27721828, 222560.20067474, 110568.82718179])
def botPost(): positions = list() try: users_data = request.json['usersLocationList'] except: return 'Ups, algo salió mal' if (len(users_data) <= 1): return 'Ups, parece que estás solo' else: for pos in users_data: positions.append( nv.lat_lon2n_E(radians(float(pos.get('lat'))), radians(float(pos.get('lon'))))) lat, lon = getMiddlePointFromList(positions) #location = getLocationName(lat,lon) bars = getPatagoniaNearbyBars(lat, lon) text = 'Encontrémonos en _{0}_ ({1}) a disfrutar una rica *Cerveza Patagonia*. ¡Nos queda cómodo a todos!'.format( bars[0]['name'], bars[0]['url']) logger.debug('botPost: %s', text) geoJsonData = createJs(users_data, {'lat': lat, 'lon': lon}, bars[0]) db_cur.execute( "INSERT INTO usrQuery(dateQuery,jsonQuery,jsonResp,groupSize,barName) VALUES (?,?,?,?,?)", [ str(datetime.now()), str(users_data), str(geoJsonData), len(users_data), bars[0]['name'] ]) db_conn.commit() return text
def nv(self): if self._nv is None: self._nv = lat_lon2n_E(deg2rad(self.lat), deg2rad(self.lng)) return self._nv
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]