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_ })