def __calcReceptorPositions(self):
w = 7.2921151467e-5
## Coordenadas aproximadas del observador
apx_pos = self.observation.getHeader()['APX_COORDS']
## Recorremos las épocas
Xest, Yest, Zest = apx_pos
corr_reloj = 0
for epoc in self.epocas:
## Coordenadas aproximadas del observador que se actualizarán
## Hora de la observación
tobs = epoc['EPOCA']
#print(tobs, sat)
## Hora de la observación en segundos GPS
tsec_week = Utils.UTC2GPS(tobs)
A = []
K = []
P = []
cont = 0
corr_reloj_ant = 1
#for i in range(0, 6):
while (abs(corr_reloj_ant - corr_reloj) > 1e-07):
##print(tobs, corr_reloj_ant, corr_reloj, corr_reloj_ant - corr_reloj, Xest, Yest, Zest)
cont += 1
##print(cont)
##if cont > 10 : break
corr_reloj_ant = corr_reloj
## Recorremos las observaciones a cada satélite
for sat in epoc['OBSERVACIONES'].keys():
## Coordenadas ECEF del satélite
Xsat, Ysat, Zsat = epoc['OBSERVACIONES'][sat]['ECEF']
## TravelTime
traveltime = sqrt((Xsat - Xest)**2 + (Ysat - Yest)**2 +
(Zsat - Zest)**2) / self.C
## OmegaTau
wt = w * traveltime
##Coordenadas del satélite rotadas
Xsat, Ysat, Zsat = [
cos(wt) * Xsat + sin(wt) * Ysat,
-sin(wt) * Xsat + cos(wt) * Ysat, Zsat
]
## Incremento de coordenadas ECEF
incX, incY, incZ = [Xsat - Xest, Ysat - Yest, Zsat - Zest]
#print(incX, incY, incZ)
## Coordenadas de la estación en lat, lon, h (Elipsoide WGS 84)
pgeo = Cargeo2Geo(Punto3D(*apx_pos), 'WGS 84')
lon_est = pgeo.getLongitud()
lat_est = pgeo.getLatitud()
h_est = pgeo.getAlturaElipsoidal()
## Incrementos de coordenadas en el Sistema Geodésico Local
eij, nij, uij = Utils.XYZ2ENU(lon_est * pi / 180,
lat_est * pi / 180, incX,
incY, incZ)
azi, ele, dist = Utils.ENU2CLA(eij, nij, uij)
##print(ele*180/pi)
## Si la elvación es menor que 10º descartamos la observación
if ele < 10 * pi / 180 or ele > 170 * pi / 180: continue
## Cálculo de la presión
pres = 1013.25 * ((1 - 0.000065 * h_est)**5.225)
## Cálculo de la temperatura
temp = 291.15 - (0.0065 * h_est)
## Humedad en la que se aproxima la altura Hortométrica
## por la altura elipsoidal
H = 50 * exp(-0.0006396 * h_est)
## Vapor de agua
e_wp = (H * 0.01) * exp(-37.2465 + 0.213166 * temp -
(0.000256908 * temp * temp))
b = self.getB(h_est)
dtropo = (0.002277 / cos(
(pi / 2) - ele)) * (pres +
((1255 / temp) + 0.05) * e_wp -
(b * (tan((pi / 2) - ele)**2)))
e_semicirculos = (ele * 180 / pi) / 180
psi = (0.0137 / (e_semicirculos + 0.11)) - 0.022
lat_semicirculos = lat_est / 180
lat_i = (lat_semicirculos + (psi * cos(azi)))
if lat_i > 0.416: lat_i = 0.416
if lat_i < -0.416: lat_i = -0.416
lon_semicirculos = lon_est / 180
lon_i = lon_semicirculos + (
(psi * sin(azi)) / cos(lat_i * pi))
lat_geomagnetica = lat_i + (0.064 * cos(
(lon_i - 1.617) * pi))
tiempo_local = (43200 * lon_i) + tsec_week
if tiempo_local >= 86400: tiempo_local -= 86400
elif tiempo_local < 0: tiempo_local += 86400
## Cálculo de la amplitud del retraso ionosférico
ion_alpha = self.navigation.getHeader()['ION_ALPHA']
ion_beta = self.navigation.getHeader()['ION_BETA']
Ai = 0
Pi = 0
for i in range(0, 4):
Ai += ion_alpha[i] * (lat_geomagnetica**i)
Pi += ion_beta[i] * (lat_geomagnetica**i)
if Ai < 0: Ai = 0
if Pi < 72000: Pi = 72000
## Cálculo de la fase del retraso ionosférico
Xi = (2 * pi * (tiempo_local - 50400) / Pi)
## Cálculo del factor de inclinación
F = 1.0 + (16.0 * (0.53 - e_semicirculos)**3)
## Cálculo del retraso ionosférico (en segundos)
if Xi <= 1.57:
Il1 = (5e-9 + (Ai * (1 - ((Xi**2) / 2) +
((Xi**4) / 24)))) * F
else:
Il1 = 5e-9 * F
## Transformación a metros del retraso ionosférico
Il1m = Il1 * self.C
## Cálculo del retraso ionosférico para la frecuencia L2 (en metros)
Il2 = 1.65 * Il1m
dist = sqrt((Xsat - Xest)**2 + (Ysat - Yest)**2 +
(Zsat - Zest)**2)
p2 = epoc['OBSERVACIONES'][sat]['OBSERVACION']['P2'][
'VALUE']
tcorr = epoc['OBSERVACIONES'][sat]['TCORR']
A.append([
-(Xsat - Xest) / dist, -(Ysat - Yest) / dist,
-(Zsat - Zest) / dist, 1
])
K.append([
p2 - dist - corr_reloj + (self.C * tcorr) - dtropo -
Il2
])
P.append((sin(ele)**2) / ((1.5 * 0.3)**2))
'''
if sat == 'G04' and cont < 2:
print (traveltime)
print (wt)
print (Xsat, Ysat, Zsat)
print (azi*180/pi, ele*180/pi)
print (lon_est*pi/180, lat_est*pi/180)
print(eij, nij, uij)
print('helip', h_est)
print('pres', pres, 'temp', temp, 'H', H, 'e_wp', e_wp, 'e_semicirculos', e_semicirculos, 'b', b, 'dtropo', dtropo, 'psi', psi)
print('lon_i', lon_i, 'lat_i', lat_i)
print('lat_geomag', lat_geomagnetica)
print('tsec', tsec_week)
print('tiempo_local', tiempo_local)
print('Ai', Ai, 'Pi', Pi, 'Il1', Il1m, 'Il2', Il2)
print('-----------------')
'''
matrizA = matrix(A)
matrizK = matrix(K)
matrizP = matrix(diag(P))
## Calculamos la solución
matrizX = [
dx, dy, dz, corr_reloj
] = linalg.inv(matrizA.transpose() * matrizP * matrizA) * (
matrizA.transpose() * matrizP * matrizK)
Xest += dx[0, 0]
Yest += dy[0, 0]
Zest += dz[0, 0]
corr_reloj = corr_reloj[0, 0] / self.C
pgeo_ = Cargeo2Geo(Punto3D(Xest, Yest, Zest), 'WGS 84')
matrizR = dot(matrizA, matrizX) - matrizK
gdl = shape(matrizA)
est_ = float(
sqrt(dot(dot(matrizR.transpose(), matrizP), matrizR)) /
(gdl[0] - gdl[1]))
#print(est_, matrizA, matrizR, 'P', matrizP)
Qxx = dot(
linalg.inv(dot(dot(matrizA.transpose(), matrizP), matrizA)),
est_)
dx = sqrt(Qxx[0, 0])
dy = sqrt(Qxx[1, 1])
dz = sqrt(Qxx[2, 2])
dt = sqrt(Qxx[3, 3])
#print(dx, dy, dz, dt)
##print(Qxx)
self.resultados.append({
'tobs': str(tobs),
'lon': pgeo_.getLongitud(),
'lat': pgeo_.getLatitud(),
'h': pgeo_.getAlturaElipsoidal(),
'X': Xest,
'Y': Yest,
'Z': Zest,
'err': [dx, dy, dx, dt],
'est': est_
})
