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 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 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 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 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 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 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 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_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)
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 getNEDElevationAngle (self, other_point): """ Returns north-east-down elevation angle to other point, measured from tangential plane to other (self as NED origin). Parameters ---------- other_point: GeoPosition containing other position Returns ------- float elevation angle(pointing up) in degrees """ assert isinstance(other_point, GeoPosition) d_r2g = self.__geopoint.delta_to(other_point.__geopoint) # type: nv.Pvector return nv.deg(-d_r2g.elevation[0])
def getNEDAzimuth (self, other_point): """ Returns north-east-down azimuth to other point, measured from north to other (self as NED origin). Parameters ---------- other_point: GeoPosition containing other position Returns ------- float azimuth in degrees """ assert isinstance(other_point, GeoPosition) d_r2g = self.__geopoint.delta_to(other_point.__geopoint) # type: nv.Pvector return nv.deg(d_r2g.azimuth[0])
def bearing(self,gridB): _instanceTypeCheck(gridB,Grid) _dist, azia, _azib = self.__point.distance_and_azimuth(gridB.__point) return deg(azia)
def test_distance_and_azimuth(self): wgs84 = FrameE(name='WGS84') point1 = wgs84.GeoPoint(latitude=-30, longitude=0, degrees=True) point2 = wgs84.GeoPoint(latitude=29.9, longitude=179.8, degrees=True) s_12, azi1, azi2 = point1.distance_and_azimuth(point2) n_a = point1.to_nvector() n_b = point2.to_nvector() s_ab, azia, azib = nv.geodesic_distance(n_a.normal, n_b.normal, wgs84.a, wgs84.f) assert_allclose(s_12, 19989832.82761) point1 = wgs84.GeoPoint(latitude=0, longitude=0, degrees=True) point2 = wgs84.GeoPoint(latitude=0.5, longitude=179.5, degrees=True) s_12, azi1, azi2 = point1.distance_and_azimuth(point2) assert_allclose(s_12, 19936288.578965) n_a = point1.to_nvector() n_b = point2.to_nvector() s_ab, azia, azib = nv.geodesic_distance(n_a.normal, n_b.normal, wgs84.a, wgs84.f) assert_allclose(s_ab, 19936288.578965) point1 = wgs84.GeoPoint(latitude=88, longitude=0, degrees=True) point2 = wgs84.GeoPoint(latitude=89, longitude=-170, degrees=True) s_12, azi1, azi2 = point1.distance_and_azimuth(point2) assert_allclose(s_12, 333947.509468) n_a = point1.to_nvector() n_b = point2.to_nvector() s_ab, azia, azib = nv.geodesic_distance(n_a.normal, n_b.normal, wgs84.a, wgs84.f) # n_EA_E = nv.lat_lon2n_E(0,0) # n_EB_E = nv.lat_lon2n_E(*nv.rad(0.5, 179.5)) # np.allclose(nv.geodesic_distance(n_EA_E, n_EB_E), 19909099.44101977) assert_allclose(nv.deg(azi1, azi2), (-3.3309161604062467, -173.327884597742)) p3, azib = point1.displace(s_12, azi1) assert_allclose(nv.deg(azib), -173.327884597742) assert_allclose(p3.latlon_deg, (89, -170, 0)) p4, azia = point2.displace(s_12, azi2 + np.pi) assert_allclose(nv.deg(azia), -3.3309161604062467 + 180) truth = (88, 0, 0) assert_allclose(p4.latlon_deg, truth, atol=1e-12) # pylint: disable=redundant-keyword-arg # ------ greatcircle -------- s_12, azi1, azi2 = point1.distance_and_azimuth(point2, method='greatcircle') assert_allclose(s_12, 331713.817039) assert_allclose(nv.deg(azi1, azi2), (-3.330916, -173.327885)) p3, azib = point1.displace(s_12, azi1, method='greatcircle') assert_allclose(nv.deg(azib), -173.32784) assert_allclose(p3.latlon_deg, (89.000005, -169.999949, 0)) p4, azia = point2.displace(s_12, azi2 + np.pi, method='greatcircle') _assert_allclose(p4.latlon_deg, truth, atol=1e-4) # Less than 0.4 meters assert_allclose(nv.deg(azia), -3.3309161604062467 + 180)