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
