def test_llh2xyz(self): # Test of single point x, y, z = llh2xyz(hp2dec(-37.482667598), hp2dec(144.581644114), 39.6514) self.assertAlmostEqual(x, -4131654.2815, 3) self.assertAlmostEqual(y, 2896107.9738, 3) self.assertAlmostEqual(z, -3888601.3067, 3) # Test DMSAngle input x, y, z = llh2xyz(DMSAngle(-37, 48, 26.67598), DMSAngle(144, 58, 16.44114), 39.6514) self.assertAlmostEqual(x, -4131654.2815, 3) self.assertAlmostEqual(y, 2896107.9738, 3) self.assertAlmostEqual(z, -3888601.3067, 3) # Test DDMAngle input x, y, z = llh2xyz(DDMAngle(-37, 48.4445996), DDMAngle(144, 58.274019), 39.6514) self.assertAlmostEqual(x, -4131654.2815, 3) self.assertAlmostEqual(y, 2896107.9738, 3) self.assertAlmostEqual(z, -3888601.3067, 3) # Tests comparing results from DynAdjust Adjusted Coordinate File # including 109 Points Across Australia abs_path = os.path.abspath(os.path.dirname(__file__)) testdata = read_dnacoord( os.path.join(abs_path, 'resources/natadjust_rvs_example.dat')) for coord in testdata: coord.converthptodd() xcomp, ycomp, zcomp = llh2xyz(coord.lat, coord.long, coord.ell_ht) assert (abs(coord.x - xcomp) < 2e-4) assert (abs(coord.y - ycomp) < 2e-4) assert (abs(coord.z - zcomp) < 2e-4)
def mga2020_to_mga94(zone, east, north, ell_ht=False, vcv=None): """ Performs conformal transformation of Map Grid of Australia 2020 to Map Grid of Australia 1994 Coordinates using the reverse form of the GDA2020 Tech Manual v1.2 7 parameter similarity transformation parameters :param zone: Zone Number - 1 to 60 :param east: Easting (m, within 3330km of Central Meridian) :param north: Northing (m, 0 to 10,000,000m) :param ell_ht: Ellipsoid Height (m) (optional) :param vcv: Optional 3*3 numpy array in local enu units to propagate tf uncertainty :return: MGA1994 Zone, Easting, Northing, Ellipsoid Height (if none provided, returns 0), and vcv matrix """ lat, lon, psf, gridconv = grid2geo(zone, east, north) if ell_ht is False: ell_ht_in = 0 else: ell_ht_in = ell_ht if vcv is not None: vcv = vcv_local2cart(vcv, lat, lon) x94, y94, z94 = llh2xyz(lat, lon, ell_ht_in) x20, y20, z20, vcv94 = conform7(x94, y94, z94, -gda94_to_gda2020, vcv=vcv) lat, lon, ell_ht_out = xyz2llh(x20, y20, z20) if vcv94 is not None: vcv94 = vcv_cart2local(vcv94, lat, lon) if ell_ht is False: ell_ht_out = 0 hemisphere, zone20, east20, north20, psf, gridconv = geo2grid(lat, lon) return zone20, east20, north20, round(ell_ht_out, 4), vcv94
def cart(self, ellipsoid=grs80): """ Convert coordinates to Cartesian Note: If no ellipsoid height set, uses 0m. No N Value output :param ellipsoid: geodepy.constants.Ellipsoid Object (default: grs80) :return: Cartesian Coordinate :rtype: CoordCart """ if self.ell_ht: x, y, z = llh2xyz(self.lat, self.lon, self.ell_ht, ellipsoid) if self.orth_ht: # Only N Value if both Ellipsoid and Ortho Heights return CoordCart(x, y, z, self.ell_ht - self.orth_ht) else: # No Ortho Height -> No N Value return CoordCart(x, y, z) else: # No Ellipsoid Height - set to 0m for conversion x, y, z = llh2xyz(self.lat, self.lon, 0, ellipsoid) return CoordCart(x, y, z) # No Ellipsoid Height -> No N Value
def test_enu2xyz(self): MOBS_MGA2020 = (55, 321820.085, 5811181.510, 40.570) MOBS_MGA1994 = (55, 321819.594, 5811180.038, 40.659) # Convert UTM Projection Coordinates to Geographic Coordinates MOBS_GDA2020 = grid2geo(MOBS_MGA2020[0], MOBS_MGA2020[1], MOBS_MGA2020[2]) MOBS_GDA1994 = grid2geo(MOBS_MGA1994[0], MOBS_MGA1994[1], MOBS_MGA1994[2]) # Convert Geographic Coordinates to Cartesian XYZ Coordinates MOBS_GDA2020_XYZ = llh2xyz(MOBS_GDA2020[0], MOBS_GDA2020[1], MOBS_MGA2020[3]) MOBS_GDA1994_XYZ = llh2xyz(MOBS_GDA1994[0], MOBS_GDA1994[1], MOBS_MGA1994[3]) # Generate Vector Between UTM Projection Coordinates mga_vector = [ MOBS_MGA2020[1] - MOBS_MGA1994[1], MOBS_MGA2020[2] - MOBS_MGA1994[2], MOBS_MGA2020[3] - MOBS_MGA1994[3] ] # Generate Vector Between Cartesian XYZ Coordinates xyz_vector = (MOBS_GDA2020_XYZ[0] - MOBS_GDA1994_XYZ[0], MOBS_GDA2020_XYZ[1] - MOBS_GDA1994_XYZ[1], MOBS_GDA2020_XYZ[2] - MOBS_GDA1994_XYZ[2]) # Rotate UTM Projection Vector by Grid Convergence grid_dist, grid_brg = rect2polar(mga_vector[0], mga_vector[1]) local_east, local_north = polar2rect(grid_dist, grid_brg - MOBS_GDA2020[3]) local_vector = (local_east, local_north, mga_vector[2]) # Calculate XYZ Vector using Local Vector Components x, y, z = enu2xyz(MOBS_GDA2020[0], MOBS_GDA2020[1], *local_vector) self.assertAlmostEqual(x, xyz_vector[0], 4) self.assertAlmostEqual(y, xyz_vector[1], 4) self.assertAlmostEqual(z, xyz_vector[2], 4) # Calculate Local Vector using XYZ Vector Components e, n, u = xyz2enu(MOBS_GDA2020[0], MOBS_GDA2020[1], *xyz_vector) self.assertAlmostEqual(e, local_vector[0], 4) self.assertAlmostEqual(n, local_vector[1], 4) self.assertAlmostEqual(u, local_vector[2], 4)
def test_llh2xyz(self): abs_path = os.path.abspath(os.path.dirname(__file__)) testdata = read_dnacoord( os.path.join(abs_path, 'resources/natadjust_rvs_example.dat')) for coord in testdata: coord.converthptodd() xcomp, ycomp, zcomp = llh2xyz(coord.lat, coord.long, coord.ell_ht) assert (abs(coord.x - xcomp) < 2e-4) assert (abs(coord.y - ycomp) < 2e-4) assert (abs(coord.z - zcomp) < 2e-4)
def err_ellip_2_ycluster(stn): # Input: Supply a station with coordinates # and error ellipse for coordinate uncertainty # Output: xml string for point cluster (Y-type observation) x, y, z = llh2xyz(float(stn.XAxis), float(stn.YAxis), float(stn.Heights)) a = stn.aAxis / 2.44774683068 b = stn.bAxis / 2.44774683068 az = 90 - stn.ErrAz r_az = radians(az) rl = get_rl(float(stn.XAxis), float(stn.YAxis)) i_a = np.zeros([3, 3]) i_a[0, 0] = (cos(r_az) * cos(r_az) * a * a) + (b * b * sin(r_az) * sin(r_az)) i_a[0, 1] = (a * a - b * b) * cos(r_az) * sin(r_az) i_a[1, 0] = i_a[0, 1] i_a[1, 1] = (a * a * sin(r_az) * sin(r_az)) + (b * b * cos(r_az) * cos(r_az)) i_a[2, 2] = 0.000001 wt = np.matmul(np.matmul(rl, i_a), rl.transpose()) xml_str = ('<DnaMeasurement>\n' + '<Type>Y</Type>\n' + '<Ignore/>\n' + '<ReferenceFrame>GDA2020</ReferenceFrame>\n' + '<Epoch>01.01.2020</Epoch>\n' + '<Vscale>1.000</Vscale>\n' + '<Pscale>1.000</Pscale>\n' + '<Lscale>1.000</Lscale>\n' + '<Hscale>1.000</Hscale>\n' + '<Coords>XYZ</Coords>\n' + '<Total>1</Total>\n' + '<First>' + stn.Name + '</First>\n' + '<Clusterpoint>\n' + '<X>' + str(x) + '</X>\n' + '<Y>' + str(y) + '</Y>\n' + '<Z>' + str(z) + '</Z>\n' + '<SigmaXX>' + str(wt[0, 0]) + '</SigmaXX>\n' + '<SigmaXY>' + str(wt[0, 1]) + '</SigmaXY>\n' + '<SigmaXZ>' + str(wt[0, 2]) + '</SigmaXZ>\n' + '<SigmaYY>' + str(wt[1, 1]) + '</SigmaYY>\n' + '<SigmaYZ>' + str(wt[1, 2]) + '</SigmaYZ>\n' + '<SigmaZZ>' + str(wt[2, 2]) + '</SigmaZZ>\n' + '</Clusterpoint>\n' + '</DnaMeasurement>\n') return xml_str
def xyz(self): return gc.llh2xyz(self.lat, self.lon, self.ehgt)
def gnss_xyz2dah(coords, xyz_bases): dah_bases = {} print(' Calculating baseline transformations ') for b in xyz_bases: f_x, f_y, f_z = llh2xyz(coords[xyz_bases[b]['first']]['LAT'], coords[xyz_bases[b]['first']]['LON'], coords[xyz_bases[b]['first']]['EHGT']) Calc_point_x = f_x + xyz_bases[b]['dX'] Calc_point_y = f_y + xyz_bases[b]['dY'] Calc_point_z = f_z + xyz_bases[b]['dZ'] p, l, h = xyz2llh(Calc_point_x, Calc_point_y, Calc_point_z) e_dist, b_az, b_rev_az = vincinv(coords[xyz_bases[b]['first']]['LAT'], coords[xyz_bases[b]['first']]['LON'], p, l) adj_dist, adj_az, b_rev_az = vincinv( coords[xyz_bases[b]['first']]['LAT'], coords[xyz_bases[b]['first']]['LON'], coords[xyz_bases[b]['second']]['LAT'], coords[xyz_bases[b]['second']]['LON']) e_dist_res = adj_dist - e_dist b_az_res = adj_az - b_az e_ht_diff = h - coords[xyz_bases[b]['first']]['EHGT'] n1 = coords[xyz_bases[b]['first']]['OHGT'] - coords[xyz_bases[b] ['first']]['EHGT'] n2 = coords[xyz_bases[b]['second']]['OHGT'] - coords[ xyz_bases[b]['second']]['EHGT'] o_ht_diff = -1 * n1 + e_ht_diff + n2 o_ht_diff_res = (coords[xyz_bases[b]['second']]['OHGT'] - coords[xyz_bases[b]['first']]['OHGT']) - o_ht_diff #transfor the baseline to distance, azimuth, up #translated code from http://peas13.dli.wa.gov.au/svn-intsvc/Delphi/GesmarComponents/trunk/Geodetic.pas m_lat = (coords[xyz_bases[b]['first']]['LAT'] + p) / 2 m_x = (f_x + Calc_point_x) / 2 m_y = (f_y + Calc_point_y) / 2 dR = sqrt(m_x * m_x + m_y * m_y) dE2 = grs80.ecc1sq dQ = radians(m_lat) sinQ = sin(dQ) dTemp = sqrt(1.0 - dE2 * sinQ * sinQ) dN = grs80.semimaj / dTemp dM = dN * ((1.0 - dE2) / (dTemp * dTemp)) sinQ = sin(dQ) cosQ = cos(dQ) secQ = 1.0 / cosQ dF = grs80.f dE2 = 2 * dF - (dF * dF) dA = m_x * tan(dQ) / (dR * dR * (dE2 - secQ * secQ)) dB = 1.0 / (dR * secQ * secQ - dE2 * dN * cosQ) dC = dR * sinQ / (cosQ * cosQ) - dN * dE2 * sinQ * cosQ / ( 1.0 - dE2 * sinQ * sinQ) JMatrix = np.zeros([3, 3]) JMatrix[0, 0] = dM * dA JMatrix[0, 1] = dM * (m_y * dA / m_x) JMatrix[0, 2] = dM * dB JMatrix[1, 0] = -(dN * cosQ) * m_y / (dR * dR) JMatrix[1, 1] = (dN * cosQ) * m_x / (dR * dR) JMatrix[1, 2] = 0.0 JMatrix[2, 0] = m_x / (dR * cosQ) + dA * dC JMatrix[2, 1] = m_y / (dR * cosQ) + m_y * dA * dC / m_x JMatrix[2, 2] = dB * dC b_var_matrix = np.zeros([3, 3]) b_var_matrix[0, 0] = xyz_bases[b]['sdev_x']**2 b_var_matrix[1, 1] = xyz_bases[b]['sdev_y']**2 b_var_matrix[2, 2] = xyz_bases[b]['sdev_z']**2 b_nst_matrix = np.zeros([3, 3]) b_nst_matrix[0, 0] = xyz_bases[b]['nstat_x']**2 b_nst_matrix[1, 1] = xyz_bases[b]['nstat_y']**2 b_nst_matrix[2, 2] = xyz_bases[b]['nstat_z']**2 cosAz = cos(radians(b_az)) sinAz = sin(radians(b_az)) AMatrix = np.zeros([3, 3]) AMatrix[0, 0] = cosAz AMatrix[0, 1] = sinAz if e_dist == 0: AMatrix[1, 0] = degrees(sinAz / 0.00001) AMatrix[1, 1] = -degrees(cosAz / 0.00001) else: AMatrix[1, 0] = degrees(sinAz / e_dist) AMatrix[1, 1] = -degrees(cosAz / e_dist) GMatrix = np.matmul(np.matmul(JMatrix, b_var_matrix), JMatrix.transpose()) dah_Matrix = np.matmul(np.matmul(AMatrix, GMatrix), AMatrix.transpose()) n_GMatrix = np.matmul(np.matmul(JMatrix, b_nst_matrix), JMatrix.transpose()) nstat_Matrix = np.matmul(np.matmul(AMatrix, n_GMatrix), AMatrix.transpose()) dah_bases[b] = { 'first': xyz_bases[b]['first'], 'second': xyz_bases[b]['second'], 'dX': xyz_bases[b]['dX'], 'dY': xyz_bases[b]['dY'], 'dZ': xyz_bases[b]['dZ'], 'e_dist': e_dist, 'e_dist_res': e_dist_res, 'e_dist_sdev': sqrt(abs(dah_Matrix[0, 0])), 'e_dist_nstat': sqrt(abs(nstat_Matrix[0, 0])), 'b_az': b_az, 'b_az_res': b_az_res, 'b_az_sdev': sqrt(abs(dah_Matrix[1, 1])), 'b_az_nstat': sqrt(abs(sin(radians(nstat_Matrix[1, 1])) * e_dist)), 'o_ht_diff': o_ht_diff, 'e_ht_diff': e_ht_diff, 'o_ht_diff_res': o_ht_diff_res, 'e_ht_diff_sdev': sqrt(abs(GMatrix[2, 2])), 'e_ht_diff_nstat': sqrt(abs(n_GMatrix[2, 2])) } return coords, dah_bases