1196 lines
31 KiB
Python
1196 lines
31 KiB
Python
import numpy as np
|
|
|
|
|
|
# cart2geo.m
|
|
|
|
|
|
def cart2geo(X, Y, Z, i, *args, **kwargs):
|
|
# CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
|
|
# coordinates (phi, lambda, h) on a selected reference ellipsoid.
|
|
|
|
# [phi, lambda, h] = cart2geo(X, Y, Z, i);
|
|
|
|
# Choices i of Reference Ellipsoid for Geographical Coordinates
|
|
# 1. International Ellipsoid 1924
|
|
# 2. International Ellipsoid 1967
|
|
# 3. World Geodetic System 1972
|
|
# 4. Geodetic Reference System 1980
|
|
# 5. World Geodetic System 1984
|
|
|
|
# Kai Borre 10-13-98
|
|
# Copyright (c) by Kai Borre
|
|
# Revision: 1.0 Date: 1998/10/23
|
|
|
|
# ==========================================================================
|
|
|
|
a = np.array([6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0])
|
|
|
|
f = np.array([1 / 297, 1 / 298.247, 1 / 298.26, 1 / 298.257222101, 1 / 298.257223563])
|
|
|
|
lambda_ = np.arctan2(Y, X)
|
|
|
|
ex2 = (2 - f[i]) * f[i] / ((1 - f[i]) ** 2)
|
|
|
|
c = a[i] * np.sqrt(1 + ex2)
|
|
|
|
phi = np.arctan(Z / (np.sqrt(X ** 2 + Y ** 2) * (1 - (2 - f[i])) * f[i]))
|
|
|
|
h = 0.1
|
|
|
|
oldh = 0
|
|
|
|
iterations = 0
|
|
|
|
while abs(h - oldh) > 1e-12:
|
|
|
|
oldh = h
|
|
|
|
N = c / np.sqrt(1 + ex2 * np.cos(phi) ** 2)
|
|
|
|
phi = np.arctan(Z / (np.sqrt(X ** 2 + Y ** 2) * (1 - (2 - f[i]) * f[i] * N / (N + h))))
|
|
|
|
h = np.sqrt(X ** 2 + Y ** 2) / np.cos(phi) - N
|
|
|
|
iterations += 1
|
|
|
|
if iterations > 100:
|
|
print ('Failed to approximate h with desired precision. h-oldh: %e.' % (h - oldh))
|
|
break
|
|
|
|
phi *= (180 / np.pi)
|
|
|
|
# b = zeros(1,3);
|
|
# b(1,1) = fix(phi);
|
|
# b(2,1) = fix(rem(phi,b(1,1))*60);
|
|
# b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600;
|
|
|
|
lambda_ *= (180 / np.pi)
|
|
|
|
# l = zeros(1,3);
|
|
# l(1,1) = fix(lambda);
|
|
# l(2,1) = fix(rem(lambda,l(1,1))*60);
|
|
# l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600;
|
|
|
|
# fprintf('\n phi =#3.0f #3.0f #8.5f',b(1),b(2),b(3))
|
|
# fprintf('\n lambda =#3.0f #3.0f #8.5f',l(1),l(2),l(3))
|
|
# fprintf('\n h =#14.3f\n',h)
|
|
return phi, lambda_, h
|
|
|
|
|
|
############## end cart2geo.m ###################
|
|
|
|
|
|
# clsin.m
|
|
def clsin(ar, degree, argument, *args, **kwargs):
|
|
# Clenshaw summation of sinus of argument.
|
|
|
|
# result = clsin(ar, degree, argument);
|
|
|
|
# Written by Kai Borre
|
|
# December 20, 1995
|
|
|
|
# See also WGS2UTM or CART2UTM
|
|
|
|
# ==========================================================================
|
|
|
|
cos_arg = 2 * np.cos(argument)
|
|
|
|
hr1 = 0
|
|
|
|
hr = 0
|
|
|
|
# TODO fix index range of t
|
|
for t in range(degree, 0, -1):
|
|
hr2 = hr1
|
|
|
|
hr1 = hr
|
|
|
|
hr = ar[t - 1] + cos_arg * hr1 - hr2
|
|
|
|
result = hr * np.sin(argument)
|
|
return result
|
|
|
|
|
|
####################### end clsin.m #####################
|
|
|
|
|
|
# clksin.m
|
|
def clksin(ar, degree, arg_real, arg_imag, *args, **kwargs):
|
|
# Clenshaw summation of sinus with complex argument
|
|
# [re, im] = clksin(ar, degree, arg_real, arg_imag);
|
|
|
|
# Written by Kai Borre
|
|
# December 20, 1995
|
|
|
|
# See also WGS2UTM or CART2UTM
|
|
|
|
# ==========================================================================
|
|
|
|
sin_arg_r = np.sin(arg_real)
|
|
|
|
cos_arg_r = np.cos(arg_real)
|
|
|
|
sinh_arg_i = np.sinh(arg_imag)
|
|
|
|
cosh_arg_i = np.cosh(arg_imag)
|
|
|
|
r = 2 * cos_arg_r * cosh_arg_i
|
|
|
|
i = - 2 * sin_arg_r * sinh_arg_i
|
|
|
|
hr1 = 0
|
|
|
|
hr = 0
|
|
|
|
hi1 = 0
|
|
|
|
hi = 0
|
|
|
|
# TODO fix index range of t
|
|
for t in range(degree, 0, - 1):
|
|
hr2 = hr1
|
|
|
|
hr1 = hr
|
|
|
|
hi2 = hi1
|
|
|
|
hi1 = hi
|
|
|
|
z = ar[t - 1] + r * hr1 - i * hi - hr2
|
|
|
|
hi = i * hr1 + r * hi1 - hi2
|
|
|
|
hr = z
|
|
|
|
r = sin_arg_r * cosh_arg_i
|
|
|
|
i = cos_arg_r * sinh_arg_i
|
|
|
|
re = r * hr - i * hi
|
|
|
|
im = r * hi + i * hr
|
|
return re, im
|
|
|
|
|
|
# cart2utm.m
|
|
def cart2utm(X, Y, Z, zone, *args, **kwargs):
|
|
# CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'.
|
|
|
|
# [E, N, U] = cart2utm(X, Y, Z, zone);
|
|
|
|
# Inputs:
|
|
# X,Y,Z - Cartesian coordinates. Coordinates are referenced
|
|
# with respect to the International Terrestrial Reference
|
|
# Frame 1996 (ITRF96)
|
|
# zone - UTM zone of the given position
|
|
|
|
# Outputs:
|
|
# E, N, U - UTM coordinates (Easting, Northing, Uping)
|
|
|
|
# Kai Borre -11-1994
|
|
# Copyright (c) by Kai Borre
|
|
|
|
# This implementation is based upon
|
|
# O. Andersson & K. Poder (1981) Koordinattransformationer
|
|
# ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
|
|
# Vol. 30: 552--571 and Vol. 31: 76
|
|
|
|
# An excellent, general reference (KW) is
|
|
# R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
|
|
# h\"oheren Geod\"asie und Kartographie.
|
|
# Erster Band, Springer Verlag
|
|
|
|
# Explanation of variables used:
|
|
# f flattening of ellipsoid
|
|
# a semi major axis in m
|
|
# m0 1 - scale at central meridian; for UTM 0.0004
|
|
# Q_n normalized meridian quadrant
|
|
# E0 Easting of central meridian
|
|
# L0 Longitude of central meridian
|
|
# bg constants for ellipsoidal geogr. to spherical geogr.
|
|
# gb constants for spherical geogr. to ellipsoidal geogr.
|
|
# gtu constants for ellipsoidal N, E to spherical N, E
|
|
# utg constants for spherical N, E to ellipoidal N, E
|
|
# tolutm tolerance for utm, 1.2E-10*meridian quadrant
|
|
# tolgeo tolerance for geographical, 0.00040 second of arc
|
|
|
|
# B, L refer to latitude and longitude. Southern latitude is negative
|
|
# International ellipsoid of 1924, valid for ED50
|
|
|
|
a = 6378388.0
|
|
|
|
f = 1.0 / 297.0
|
|
|
|
ex2 = (2 - f) * f / (1 - f) ** 2
|
|
|
|
c = a * np.sqrt(1 + ex2)
|
|
|
|
vec = np.array([X, Y, Z - 4.5])
|
|
|
|
alpha = 7.56e-07
|
|
|
|
R = np.array([[1, - alpha, 0],
|
|
[alpha, 1, 0],
|
|
[0, 0, 1]])
|
|
|
|
trans = np.array([89.5, 93.8, 127.6])
|
|
|
|
scale = 0.9999988
|
|
|
|
v = scale * R.dot(vec) + trans
|
|
|
|
L = np.arctan2(v[1], v[0])
|
|
|
|
N1 = 6395000.0
|
|
|
|
B = np.arctan2(v[2] / ((1 - f) ** 2 * N1), np.linalg.norm(v[0:2]) / N1)
|
|
|
|
U = 0.1
|
|
|
|
oldU = 0
|
|
|
|
iterations = 0
|
|
|
|
while abs(U - oldU) > 0.0001:
|
|
|
|
oldU = U
|
|
|
|
N1 = c / np.sqrt(1 + ex2 * (np.cos(B)) ** 2)
|
|
|
|
B = np.arctan2(v[2] / ((1 - f) ** 2 * N1 + U), np.linalg.norm(v[0:2]) / (N1 + U))
|
|
|
|
U = np.linalg.norm(v[0:2]) / np.cos(B) - N1
|
|
|
|
iterations += 1
|
|
|
|
if iterations > 100:
|
|
print ('Failed to approximate U with desired precision. U-oldU: %e.' % (U - oldU))
|
|
break
|
|
|
|
# Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
|
|
m0 = 0.0004
|
|
|
|
n = f / (2 - f)
|
|
|
|
m = n ** 2 * (1.0 / 4.0 + n ** 2 / 64)
|
|
|
|
w = (a * (-n - m0 + m * (1 - m0))) / (1 + n)
|
|
|
|
Q_n = a + w
|
|
|
|
# Easting and longitude of central meridian
|
|
E0 = 500000.0
|
|
|
|
L0 = (zone - 30) * 6 - 3
|
|
|
|
# Check tolerance for reverse transformation
|
|
tolutm = np.pi / 2 * 1.2e-10 * Q_n
|
|
|
|
tolgeo = 4e-05
|
|
|
|
# Coefficients of trigonometric series
|
|
|
|
# ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52)
|
|
# bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45))));
|
|
# bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9)));
|
|
# bg[3] = n^3*(-26/15 + n*34/21);
|
|
# bg[4] = n^4*1237/630;
|
|
|
|
# spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62)
|
|
# gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45)));
|
|
# gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45)));
|
|
# gb[3] = n^3*(56/15 + n*(-136/35));
|
|
# gb[4] = n^4*4279/630;
|
|
|
|
# spherical to ellipsoidal N, E, KW p. 196, (69)
|
|
# gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180)));
|
|
# gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440));
|
|
# gtu[3] = n^3*(61/240 + n*(-103/140));
|
|
# gtu[4] = n^4*49561/161280;
|
|
|
|
# ellipsoidal to spherical N, E, KW p. 194, (65)
|
|
# utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360)));
|
|
# utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440));
|
|
# utg[3] = n^3*(-17/480 + n*37/840);
|
|
# utg[4] = n^4*(-4397/161280);
|
|
|
|
# With f = 1/297 we get
|
|
|
|
bg = np.array([- 0.00337077907, 4.73444769e-06, -8.2991457e-09, 1.5878533e-11])
|
|
|
|
gb = np.array([0.00337077588, 6.6276908e-06, 1.78718601e-08, 5.49266312e-11])
|
|
|
|
gtu = np.array([0.000841275991, 7.67306686e-07, 1.2129123e-09, 2.48508228e-12])
|
|
|
|
utg = np.array([-0.000841276339, -5.95619298e-08, -1.69485209e-10, -2.20473896e-13])
|
|
|
|
# Ellipsoidal latitude, longitude to spherical latitude, longitude
|
|
neg_geo = False
|
|
|
|
if B < 0:
|
|
neg_geo = True
|
|
|
|
Bg_r = np.abs(B)
|
|
|
|
res_clensin = clsin(bg, 4, 2 * Bg_r)
|
|
|
|
Bg_r = Bg_r + res_clensin
|
|
|
|
L0 = L0 * np.pi / 180
|
|
|
|
Lg_r = L - L0
|
|
|
|
# Spherical latitude, longitude to complementary spherical latitude
|
|
# i.e. spherical N, E
|
|
cos_BN = np.cos(Bg_r)
|
|
|
|
Np = np.arctan2(np.sin(Bg_r), np.cos(Lg_r) * cos_BN)
|
|
|
|
Ep = np.arctanh(np.sin(Lg_r) * cos_BN)
|
|
|
|
# Spherical normalized N, E to ellipsoidal N, E
|
|
Np *= 2
|
|
|
|
Ep *= 2
|
|
|
|
dN, dE = clksin(gtu, 4, Np, Ep)
|
|
|
|
Np /= 2
|
|
|
|
Ep /= 2
|
|
|
|
Np += dN
|
|
|
|
Ep += dE
|
|
|
|
N = Q_n * Np
|
|
|
|
E = Q_n * Ep + E0
|
|
|
|
if neg_geo:
|
|
N = -N + 20000000
|
|
return E, N, U
|
|
|
|
|
|
#################### end cart2utm.m ####################
|
|
|
|
|
|
# deg2dms.m
|
|
def deg2dms(deg, *args, **kwargs):
|
|
# DEG2DMS Conversion of degrees to degrees, minutes, and seconds.
|
|
# The output format (dms format) is: (degrees*100 + minutes + seconds/100)
|
|
|
|
# Written by Kai Borre
|
|
# February 7, 2001
|
|
# Updated by Darius Plausinaitis
|
|
|
|
# Save the sign for later processing
|
|
neg_arg = False
|
|
|
|
if deg < 0:
|
|
# Only positive numbers should be used while spliting into deg/min/sec
|
|
deg = -deg
|
|
|
|
neg_arg = True
|
|
|
|
# Split degrees minutes and seconds
|
|
int_deg = np.floor(deg)
|
|
|
|
decimal = deg - int_deg
|
|
|
|
min_part = decimal * 60
|
|
|
|
min_ = np.floor(min_part)
|
|
|
|
sec_part = min_part - np.floor(min_part)
|
|
|
|
sec = sec_part * 60
|
|
|
|
# Check for overflow
|
|
if sec == 60.0:
|
|
min_ = min_ + 1
|
|
|
|
sec = 0.0
|
|
|
|
if min_ == 60.0:
|
|
int_deg = int_deg + 1
|
|
|
|
min_ = 0.0
|
|
|
|
# Construct the output
|
|
dmsOutput = int_deg * 100 + min_ + sec / 100
|
|
|
|
# Correct the sign
|
|
if neg_arg:
|
|
dmsOutput = -dmsOutput
|
|
return dmsOutput
|
|
|
|
|
|
################### end deg2dms.m ################
|
|
|
|
|
|
# dms2mat.m
|
|
def dms2mat(dmsInput, n, *args, **kwargs):
|
|
# DMS2MAT Splits a real a = dd*100 + mm + s/100 into[dd mm s.ssss]
|
|
# where n specifies the power of 10, to which the resulting seconds
|
|
# of the output should be rounded. E.g.: if a result is 23.823476
|
|
# seconds, and n = -3, then the output will be 23.823.
|
|
|
|
# Written by Kai Borre
|
|
# January 7, 2007
|
|
# Updated by Darius Plausinaitis
|
|
|
|
neg_arg = False
|
|
|
|
if dmsInput < 0:
|
|
# Only positive numbers should be used while spliting into deg/min/sec
|
|
dmsInput = -dmsInput
|
|
|
|
neg_arg = True
|
|
|
|
# Split degrees minutes and seconds
|
|
int_deg = np.floor(dmsInput / 100)
|
|
|
|
mm = np.floor(dmsInput - 100 * int_deg)
|
|
|
|
# we assume n<7; hence #2.10f is sufficient to hold ssdec
|
|
ssdec = '%2.10f' % (dmsInput - 100 * int_deg - mm) * 100
|
|
|
|
# Check for overflow
|
|
if ssdec == 60.0:
|
|
mm = mm + 1
|
|
|
|
ssdec = 0.0
|
|
|
|
if mm == 60.0:
|
|
int_deg = int_deg + 1
|
|
|
|
mm = 0.0
|
|
|
|
# Corect the sign
|
|
if neg_arg:
|
|
int_deg = -int_deg
|
|
|
|
# Compose the output
|
|
matOutput = []
|
|
matOutput[0] = int_deg
|
|
|
|
matOutput[1] = mm
|
|
|
|
matOutput[2] = float(ssdec[0:- n + 3])
|
|
|
|
return matOutput
|
|
|
|
|
|
################### end dms2mat.m ################
|
|
|
|
|
|
# e_r_corr.m
|
|
|
|
|
|
def e_r_corr(traveltime, X_sat, *args, **kwargs):
|
|
# E_R_CORR Returns rotated satellite ECEF coordinates due to Earth
|
|
# rotation during signal travel time
|
|
|
|
# X_sat_rot = e_r_corr(traveltime, X_sat);
|
|
|
|
# Inputs:
|
|
# travelTime - signal travel time
|
|
# X_sat - satellite's ECEF coordinates
|
|
|
|
# Outputs:
|
|
# X_sat_rot - rotated satellite's coordinates (ECEF)
|
|
|
|
# Written by Kai Borre
|
|
# Copyright (c) by Kai Borre
|
|
|
|
# ==========================================================================
|
|
|
|
Omegae_dot = 7.292115147e-05
|
|
|
|
# --- Find rotation angle --------------------------------------------------
|
|
omegatau = Omegae_dot * traveltime
|
|
|
|
# --- Make a rotation matrix -----------------------------------------------
|
|
R3 = np.array([[np.cos(omegatau), np.sin(omegatau), 0.0],
|
|
[-np.sin(omegatau), np.cos(omegatau), 0.0],
|
|
[0.0, 0.0, 1.0]])
|
|
|
|
# --- Do the rotation ------------------------------------------------------
|
|
X_sat_rot = R3.dot(X_sat)
|
|
return X_sat_rot
|
|
|
|
|
|
######## end e_r_corr.m ####################
|
|
|
|
# findUtmZone.m
|
|
|
|
|
|
def findUtmZone(latitude, longitude, *args, **kwargs):
|
|
# Function finds the UTM zone number for given longitude and latitude.
|
|
# The longitude value must be between -180 (180 degree West) and 180 (180
|
|
# degree East) degree. The latitude must be within -80 (80 degree South) and
|
|
# 84 (84 degree North).
|
|
|
|
# utmZone = findUtmZone(latitude, longitude);
|
|
|
|
# Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
|
|
# 15 deg 30 min).
|
|
|
|
# Check value bounds =====================================================
|
|
|
|
if longitude > 180 or longitude < - 180:
|
|
raise IOError('Longitude value exceeds limits (-180:180).')
|
|
|
|
if latitude > 84 or latitude < - 80:
|
|
raise IOError('Latitude value exceeds limits (-80:84).')
|
|
|
|
# Find zone ==============================================================
|
|
|
|
# Start at 180 deg west = -180 deg
|
|
|
|
utmZone = np.fix((180 + longitude) / 6) + 1
|
|
|
|
# Correct zone numbers for particular areas ==============================
|
|
|
|
if latitude > 72:
|
|
# Corrections for zones 31 33 35 37
|
|
if 0 <= longitude < 9:
|
|
utmZone = 31
|
|
|
|
elif 9 <= longitude < 21:
|
|
utmZone = 33
|
|
|
|
elif 21 <= longitude < 33:
|
|
utmZone = 35
|
|
|
|
elif 33 <= longitude < 42:
|
|
utmZone = 37
|
|
|
|
elif 56 <= latitude < 64:
|
|
# Correction for zone 32
|
|
if 3 <= longitude < 12:
|
|
utmZone = 32
|
|
return utmZone
|
|
|
|
|
|
# geo2cart.m
|
|
def geo2cart(phi, lambda_, h, i=4, *args, **kwargs):
|
|
# GEO2CART Conversion of geographical coordinates (phi, lambda, h) to
|
|
# Cartesian coordinates (X, Y, Z).
|
|
|
|
# [X, Y, Z] = geo2cart(phi, lambda, h, i);
|
|
|
|
# Format for phi and lambda: [degrees minutes seconds].
|
|
# h, X, Y, and Z are in meters.
|
|
|
|
# Choices i of Reference Ellipsoid
|
|
# 1. International Ellipsoid 1924
|
|
# 2. International Ellipsoid 1967
|
|
# 3. World Geodetic System 1972
|
|
# 4. Geodetic Reference System 1980
|
|
# 5. World Geodetic System 1984
|
|
|
|
# Inputs:
|
|
# phi - geocentric latitude (format [degrees minutes seconds])
|
|
# lambda - geocentric longitude (format [degrees minutes seconds])
|
|
# h - height
|
|
# i - reference ellipsoid type
|
|
|
|
# Outputs:
|
|
# X, Y, Z - Cartesian coordinates (meters)
|
|
|
|
# Kai Borre 10-13-98
|
|
# Copyright (c) by Kai Borre
|
|
|
|
# ==========================================================================
|
|
|
|
b = phi[0] + phi[1] / 60. + phi[2] / 3600.
|
|
|
|
b = b * np.pi / 180.
|
|
|
|
l = lambda_[1] + lambda_[2] / 60. + lambda_[3] / 3600.
|
|
|
|
l = l * np.pi / 180.
|
|
|
|
a = [6378388, 6378160, 6378135, 6378137, 6378137]
|
|
|
|
f = [1 / 297, 1 / 298.247, 1 / 298.26, 1 / 298.257222101, 1 / 298.257223563]
|
|
|
|
ex2 = (2. - f[i]) * f[i] / (1. - f[i]) ** 2
|
|
|
|
c = a[i] * np.sqrt(1. + ex2)
|
|
|
|
N = c / np.sqrt(1. + ex2 * np.cos(b) ** 2)
|
|
|
|
X = (N + h) * np.cos(b) * np.cos(l)
|
|
|
|
Y = (N + h) * np.cos(b) * np.sin(l)
|
|
|
|
Z = ((1. - f[i]) ** 2 * N + h) * np.sin(b)
|
|
|
|
return X, Y, Z
|
|
|
|
|
|
# leastSquarePos.m
|
|
def leastSquarePos(satpos_, obs, settings, *args, **kwargs):
|
|
# Function calculates the Least Square Solution.
|
|
|
|
# [pos, el, az, dop] = leastSquarePos(satpos, obs, settings);
|
|
|
|
# Inputs:
|
|
# satpos - Satellites positions (in ECEF system: [X; Y; Z;] -
|
|
# one column per satellite)
|
|
# obs - Observations - the pseudorange measurements to each
|
|
# satellite:
|
|
# (e.g. [20000000 21000000 .... .... .... .... ....])
|
|
# settings - receiver settings
|
|
|
|
# Outputs:
|
|
# pos - receiver position and receiver clock error
|
|
# (in ECEF system: [X, Y, Z, dt])
|
|
# el - Satellites elevation angles (degrees)
|
|
# az - Satellites azimuth angles (degrees)
|
|
# dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
|
|
|
|
# === Initialization =======================================================
|
|
nmbOfIterations = 7
|
|
|
|
dtr = np.pi / 180
|
|
|
|
pos = np.zeros(4)
|
|
|
|
X = satpos_.copy()
|
|
|
|
nmbOfSatellites = satpos_.shape[1]
|
|
|
|
A = np.zeros((nmbOfSatellites, 4))
|
|
|
|
omc = np.zeros(nmbOfSatellites)
|
|
|
|
az = np.zeros(nmbOfSatellites)
|
|
|
|
el = np.zeros(nmbOfSatellites)
|
|
|
|
dop = np.zeros(5)
|
|
# === Iteratively find receiver position ===================================
|
|
for iter_ in range(nmbOfIterations):
|
|
for i in range(nmbOfSatellites):
|
|
if iter_ == 0:
|
|
# --- Initialize variables at the first iteration --------------
|
|
Rot_X = X[:, i].copy()
|
|
|
|
trop = 2
|
|
|
|
else:
|
|
# --- Update equations -----------------------------------------
|
|
rho2 = (X[0, i] - pos[0]) ** 2 + (X[1, i] - pos[1]) ** 2 + (X[2, i] - pos[2]) ** 2
|
|
|
|
traveltime = np.sqrt(rho2) / settings.c
|
|
|
|
Rot_X = e_r_corr(traveltime, X[:, i])
|
|
|
|
az[i], el[i], dist = topocent(pos[0:3], Rot_X - pos[0:3])
|
|
|
|
if settings.useTropCorr:
|
|
# --- Calculate tropospheric correction --------------------
|
|
trop = tropo(np.sin(el[i] * dtr), 0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0)
|
|
|
|
else:
|
|
# Do not calculate or apply the tropospheric corrections
|
|
trop = 0
|
|
|
|
# --- Apply the corrections ----------------------------------------
|
|
omc[i] = obs[i] - np.linalg.norm(Rot_X - pos[0:3]) - pos[3] - trop
|
|
|
|
A[i, :] = np.array([-(Rot_X[0] - pos[0]) / obs[i],
|
|
-(Rot_X[1] - pos[1]) / obs[i],
|
|
-(Rot_X[2] - pos[2]) / obs[i],
|
|
1])
|
|
|
|
# These lines allow the code to exit gracefully in case of any errors
|
|
if np.linalg.matrix_rank(A) != 4:
|
|
pos = np.zeros((4, 1))
|
|
|
|
return pos, el, az, dop
|
|
# --- Find position update ---------------------------------------------
|
|
x = np.linalg.lstsq(A, omc, rcond=None)[0]
|
|
|
|
pos = pos + x.flatten()
|
|
|
|
pos = pos.T
|
|
|
|
# === Calculate Dilution Of Precision ======================================
|
|
# --- Initialize output ------------------------------------------------
|
|
# dop = np.zeros((1, 5))
|
|
|
|
Q = np.linalg.inv(A.T.dot(A))
|
|
|
|
dop[0] = np.sqrt(np.trace(Q))
|
|
|
|
dop[1] = np.sqrt(Q[0, 0] + Q[1, 1] + Q[2, 2])
|
|
|
|
dop[2] = np.sqrt(Q[0, 0] + Q[1, 1])
|
|
|
|
dop[3] = np.sqrt(Q[2, 2])
|
|
|
|
dop[4] = np.sqrt(Q[3, 3])
|
|
|
|
return pos, el, az, dop
|
|
|
|
|
|
# check_t.m
|
|
|
|
|
|
def check_t(time, *args, **kwargs):
|
|
# CHECK_T accounting for beginning or end of week crossover.
|
|
|
|
# corrTime = check_t(time);
|
|
|
|
# Inputs:
|
|
# time - time in seconds
|
|
|
|
# Outputs:
|
|
# corrTime - corrected time (seconds)
|
|
|
|
# Kai Borre 04-01-96
|
|
# Copyright (c) by Kai Borre
|
|
|
|
# ==========================================================================
|
|
|
|
half_week = 302400.0
|
|
|
|
corrTime = time
|
|
|
|
if time > half_week:
|
|
corrTime = time - 2 * half_week
|
|
|
|
elif time < - half_week:
|
|
corrTime = time + 2 * half_week
|
|
return corrTime
|
|
|
|
|
|
####### end check_t.m #################
|
|
|
|
|
|
# satpos.m
|
|
|
|
|
|
def satpos(transmitTime, prnList, eph, settings, *args, **kwargs):
|
|
# SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for
|
|
# given ephemeris EPH. Coordinates are computed for each satellite in the
|
|
# list PRNLIST.
|
|
# [satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings);
|
|
|
|
# Inputs:
|
|
# transmitTime - transmission time
|
|
# prnList - list of PRN-s to be processed
|
|
# eph - ephemerides of satellites
|
|
# settings - receiver settings
|
|
|
|
# Outputs:
|
|
# satPositions - position of satellites (in ECEF system [X; Y; Z;])
|
|
# satClkCorr - correction of satellite clocks
|
|
|
|
# Initialize constants ===================================================
|
|
numOfSatellites = prnList.size
|
|
|
|
# GPS constatns
|
|
|
|
gpsPi = 3.14159265359
|
|
|
|
# system
|
|
|
|
# --- Constants for satellite position calculation -------------------------
|
|
Omegae_dot = 7.2921151467e-05
|
|
|
|
GM = 3.986005e+14
|
|
|
|
# the mass of the Earth, [m^3/s^2]
|
|
F = - 4.442807633e-10
|
|
|
|
# Initialize results =====================================================
|
|
satClkCorr = np.zeros(numOfSatellites)
|
|
|
|
satPositions = np.zeros((3, numOfSatellites))
|
|
|
|
# Process each satellite =================================================
|
|
|
|
for satNr in range(numOfSatellites):
|
|
prn = prnList[satNr] - 1
|
|
|
|
# Find initial satellite clock correction --------------------------------
|
|
# --- Find time difference ---------------------------------------------
|
|
dt = check_t(transmitTime - eph[prn].t_oc)
|
|
|
|
satClkCorr[satNr] = (eph[prn].a_f2 * dt + eph[prn].a_f1) * dt + eph[prn].a_f0 - eph[prn].T_GD
|
|
|
|
time = transmitTime - satClkCorr[satNr]
|
|
|
|
# Find satellite's position ----------------------------------------------
|
|
# Restore semi-major axis
|
|
a = eph[prn].sqrtA * eph[prn].sqrtA
|
|
|
|
tk = check_t(time - eph[prn].t_oe)
|
|
|
|
n0 = np.sqrt(GM / a ** 3)
|
|
|
|
n = n0 + eph[prn].deltan
|
|
|
|
M = eph[prn].M_0 + n * tk
|
|
|
|
M = np.remainder(M + 2 * gpsPi, 2 * gpsPi)
|
|
|
|
E = M
|
|
|
|
for ii in range(10):
|
|
E_old = E
|
|
|
|
E = M + eph[prn].e * np.sin(E)
|
|
|
|
dE = np.remainder(E - E_old, 2 * gpsPi)
|
|
|
|
if abs(dE) < 1e-12:
|
|
# Necessary precision is reached, exit from the loop
|
|
break
|
|
# Reduce eccentric anomaly to between 0 and 360 deg
|
|
E = np.remainder(E + 2 * gpsPi, 2 * gpsPi)
|
|
|
|
dtr = F * eph[prn].e * eph[prn].sqrtA * np.sin(E)
|
|
|
|
nu = np.arctan2(np.sqrt(1 - eph[prn].e ** 2) * np.sin(E), np.cos(E) - eph[prn].e)
|
|
|
|
phi = nu + eph[prn].omega
|
|
|
|
phi = np.remainder(phi, 2 * gpsPi)
|
|
|
|
u = phi + eph[prn].C_uc * np.cos(2 * phi) + eph[prn].C_us * np.sin(2 * phi)
|
|
|
|
r = a * (1 - eph[prn].e * np.cos(E)) + eph[prn].C_rc * np.cos(2 * phi) + eph[prn].C_rs * np.sin(2 * phi)
|
|
|
|
i = eph[prn].i_0 + eph[prn].iDot * tk + eph[prn].C_ic * np.cos(2 * phi) + eph[prn].C_is * np.sin(2 * phi)
|
|
|
|
Omega = eph[prn].omega_0 + (eph[prn].omegaDot - Omegae_dot) * tk - Omegae_dot * eph[prn].t_oe
|
|
|
|
Omega = np.remainder(Omega + 2 * gpsPi, 2 * gpsPi)
|
|
|
|
satPositions[0, satNr] = np.cos(u) * r * np.cos(Omega) - np.sin(u) * r * np.cos(i) * np.sin(Omega)
|
|
|
|
satPositions[1, satNr] = np.cos(u) * r * np.sin(Omega) + np.sin(u) * r * np.cos(i) * np.cos(Omega)
|
|
|
|
satPositions[2, satNr] = np.sin(u) * r * np.sin(i)
|
|
|
|
# Include relativistic correction in clock correction --------------------
|
|
satClkCorr[satNr] = (eph[prn].a_f2 * dt + eph[prn].a_f1) * dt + eph[prn].a_f0 - eph[prn].T_GD + dtr
|
|
return satPositions, satClkCorr
|
|
|
|
|
|
# topocent.m
|
|
|
|
|
|
# togeod.m
|
|
def togeod(a, finv, X, Y, Z, *args, **kwargs):
|
|
# TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude,
|
|
# height given Cartesian coordinates X,Y,Z, and reference ellipsoid
|
|
# values semi-major axis (a) and the inverse of flattening (finv).
|
|
|
|
# [dphi, dlambda, h] = togeod(a, finv, X, Y, Z);
|
|
|
|
# The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
|
|
# The output units of angular quantities will be in decimal degrees
|
|
# (15.5 degrees not 15 deg 30 min). The output units of h will be the
|
|
# same as the units of X,Y,Z,a.
|
|
|
|
# Inputs:
|
|
# a - semi-major axis of the reference ellipsoid
|
|
# finv - inverse of flattening of the reference ellipsoid
|
|
# X,Y,Z - Cartesian coordinates
|
|
|
|
# Outputs:
|
|
# dphi - latitude
|
|
# dlambda - longitude
|
|
# h - height above reference ellipsoid
|
|
|
|
# Copyright (C) 1987 C. Goad, Columbus, Ohio
|
|
# Reprinted with permission of author, 1996
|
|
# Fortran code translated into MATLAB
|
|
# Kai Borre 03-30-96
|
|
|
|
# ==========================================================================
|
|
|
|
h = 0.0
|
|
|
|
tolsq = 1e-10
|
|
|
|
maxit = 10
|
|
|
|
# compute radians-to-degree factor
|
|
rtd = 180 / np.pi
|
|
|
|
# compute square of eccentricity
|
|
if finv < 1e-20:
|
|
esq = 0.0
|
|
|
|
else:
|
|
esq = (2 - 1 / finv) / finv
|
|
|
|
oneesq = 1 - esq
|
|
|
|
# first guess
|
|
# P is distance from spin axis
|
|
P = np.sqrt(X ** 2 + Y ** 2)
|
|
|
|
# direct calculation of longitude
|
|
|
|
if P > 1e-20:
|
|
dlambda = np.arctan2(Y, X) * rtd
|
|
|
|
else:
|
|
dlambda = 0.0
|
|
|
|
if dlambda < 0:
|
|
dlambda = dlambda + 360
|
|
|
|
# r is distance from origin (0,0,0)
|
|
r = np.sqrt(P ** 2 + Z ** 2)
|
|
|
|
if r > 1e-20:
|
|
sinphi = Z / r
|
|
|
|
else:
|
|
sinphi = 0.0
|
|
|
|
dphi = np.arcsin(sinphi)
|
|
|
|
# initial value of height = distance from origin minus
|
|
# approximate distance from origin to surface of ellipsoid
|
|
if r < 1e-20:
|
|
h = 0.0
|
|
|
|
return dphi, dlambda, h
|
|
|
|
h = r - a * (1 - sinphi * sinphi / finv)
|
|
|
|
# iterate
|
|
for i in range(maxit):
|
|
sinphi = np.sin(dphi)
|
|
|
|
cosphi = np.cos(dphi)
|
|
|
|
N_phi = a / np.sqrt(1 - esq * sinphi * sinphi)
|
|
|
|
dP = P - (N_phi + h) * cosphi
|
|
|
|
dZ = Z - (N_phi * oneesq + h) * sinphi
|
|
|
|
h = h + sinphi * dZ + cosphi * dP
|
|
|
|
dphi = dphi + (cosphi * dZ - sinphi * dP) / (N_phi + h)
|
|
|
|
if (dP * dP + dZ * dZ) < tolsq:
|
|
break
|
|
# Not Converged--Warn user
|
|
if i == maxit - 1:
|
|
print( ' Problem in TOGEOD, did not converge in %2.0f iterations' % i)
|
|
|
|
dphi *= rtd
|
|
return dphi, dlambda, h
|
|
|
|
|
|
######## end togeod.m ######################
|
|
|
|
|
|
def topocent(X, dx, *args, **kwargs):
|
|
# TOPOCENT Transformation of vector dx into topocentric coordinate
|
|
# system with origin at X.
|
|
# Both parameters are 3 by 1 vectors.
|
|
|
|
# [Az, El, D] = topocent(X, dx);
|
|
|
|
# Inputs:
|
|
# X - vector origin corrdinates (in ECEF system [X; Y; Z;])
|
|
# dx - vector ([dX; dY; dZ;]).
|
|
|
|
# Outputs:
|
|
# D - vector length. Units like units of the input
|
|
# Az - azimuth from north positive clockwise, degrees
|
|
# El - elevation angle, degrees
|
|
|
|
# Kai Borre 11-24-96
|
|
# Copyright (c) by Kai Borre
|
|
|
|
# ==========================================================================
|
|
|
|
dtr = np.pi / 180
|
|
|
|
phi, lambda_, h = togeod(6378137, 298.257223563, X[0], X[1], X[2])
|
|
|
|
cl = np.cos(lambda_ * dtr)
|
|
|
|
sl = np.sin(lambda_ * dtr)
|
|
|
|
cb = np.cos(phi * dtr)
|
|
|
|
sb = np.sin(phi * dtr)
|
|
|
|
F = np.array([[- sl, -sb * cl, cb * cl], [cl, -sb * sl, cb * sl], [0.0, cb, sb]])
|
|
|
|
local_vector = F.T.dot(dx)
|
|
|
|
E = local_vector[0]
|
|
|
|
N = local_vector[1]
|
|
|
|
U = local_vector[2]
|
|
|
|
hor_dis = np.sqrt(E ** 2 + N ** 2)
|
|
|
|
if hor_dis < 1e-20:
|
|
Az = 0.0
|
|
|
|
El = 90.0
|
|
|
|
else:
|
|
Az = np.arctan2(E, N) / dtr
|
|
|
|
El = np.arctan2(U, hor_dis) / dtr
|
|
|
|
if Az < 0:
|
|
Az = Az + 360
|
|
|
|
D = np.sqrt(dx[0] ** 2 + dx[1] ** 2 + dx[2] ** 2)
|
|
return Az, El, D
|
|
|
|
|
|
######### end topocent.m #########
|
|
|
|
|
|
# tropo.m
|
|
|
|
|
|
def tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum):
|
|
# TROPO Calculation of tropospheric correction.
|
|
# The range correction ddr in m is to be subtracted from
|
|
# pseudo-ranges and carrier phases
|
|
|
|
# ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum);
|
|
|
|
# Inputs:
|
|
# sinel - sin of elevation angle of satellite
|
|
# hsta - height of station in km
|
|
# p - atmospheric pressure in mb at height hp
|
|
# tkel - surface temperature in degrees Kelvin at height htkel
|
|
# hum - humidity in # at height hhum
|
|
# hp - height of pressure measurement in km
|
|
# htkel - height of temperature measurement in km
|
|
# hhum - height of humidity measurement in km
|
|
|
|
# Outputs:
|
|
# ddr - range correction (meters)
|
|
|
|
# Reference
|
|
# Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric
|
|
# Refraction Correction Model. Paper presented at the
|
|
# American Geophysical Union Annual Fall Meeting, San
|
|
# Francisco, December 12-17
|
|
|
|
# A Matlab reimplementation of a C code from driver.
|
|
# Kai Borre 06-28-95
|
|
|
|
# ==========================================================================
|
|
|
|
a_e = 6378.137
|
|
|
|
b0 = 7.839257e-05
|
|
|
|
tlapse = -6.5
|
|
|
|
tkhum = tkel + tlapse * (hhum - htkel)
|
|
|
|
atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15)
|
|
|
|
e0 = 0.0611 * hum * 10 ** atkel
|
|
|
|
tksea = tkel - tlapse * htkel
|
|
|
|
em = -978.77 / (2870400.0 * tlapse * 1e-05)
|
|
|
|
tkelh = tksea + tlapse * hhum
|
|
|
|
e0sea = e0 * (tksea / tkelh) ** (4 * em)
|
|
|
|
tkelp = tksea + tlapse * hp
|
|
|
|
psea = p * (tksea / tkelp) ** em
|
|
|
|
if sinel < 0:
|
|
sinel = 0
|
|
|
|
tropo_ = 0.0
|
|
|
|
done = False
|
|
|
|
refsea = 7.7624e-05 / tksea
|
|
|
|
htop = 1.1385e-05 / refsea
|
|
|
|
refsea = refsea * psea
|
|
|
|
ref = refsea * ((htop - hsta) / htop) ** 4
|
|
|
|
while 1:
|
|
rtop = (a_e + htop) ** 2 - (a_e + hsta) ** 2 * (1 - sinel ** 2)
|
|
|
|
if rtop < 0:
|
|
rtop = 0
|
|
|
|
rtop = np.sqrt(rtop) - (a_e + hsta) * sinel
|
|
|
|
a = -sinel / (htop - hsta)
|
|
|
|
b = -b0 * (1 - sinel ** 2) / (htop - hsta)
|
|
|
|
rn = np.zeros(8)
|
|
|
|
for i in range(8):
|
|
rn[i] = rtop ** (i + 2)
|
|
|
|
alpha = np.array([2 * a, 2 * a ** 2 + 4 * b / 3, a * (a ** 2 + 3 * b),
|
|
a ** 4 / 5 + 2.4 * a ** 2 * b + 1.2 * b ** 2,
|
|
2 * a * b * (a ** 2 + 3 * b) / 3,
|
|
b ** 2 * (6 * a ** 2 + 4 * b) * 0.1428571, 0, 0])
|
|
|
|
if b ** 2 > 1e-35:
|
|
alpha[6] = a * b ** 3 / 2
|
|
|
|
alpha[7] = b ** 4 / 9
|
|
|
|
dr = rtop
|
|
|
|
dr = dr + alpha.dot(rn)
|
|
|
|
tropo_ += dr * ref * 1000
|
|
|
|
if done:
|
|
ddr = tropo_
|
|
|
|
break
|
|
done = True
|
|
|
|
refsea = (0.3719 / tksea - 1.292e-05) / tksea
|
|
|
|
htop = 1.1385e-05 * (1255.0 / tksea + 0.05) / refsea
|
|
|
|
ref = refsea * e0sea * ((htop - hsta) / htop) ** 4
|
|
return ddr
|
|
|
|
|
|
######### end tropo.m ###################
|
|
|
|
|
|
if __name__ == '__main__':
|
|
# print "This program is being run by itself"
|
|
pass
|
|
else:
|
|
print( 'Importing functions from ./geoFunctions/')
|