ngocmx/SoftGNSS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
fee1cd14c8
SoftGNSS/geoFunctions/__init__.py

1196 lines
31 KiB
Python
Raw Normal View History

first commit
2025-10-22 16:08:12 +07:00
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/')
Reference in New Issue Copy Permalink