def standardMeteorologicalMethod(approx_position, ellipsoid, elevation): """ 标准气象元素 Saastamoinen模型 <p>对流层延迟 :param x: 坐标 :param y: :param z: :param elevation: 卫星仰角 :return: 改正后的对流层时延 """ # 从内部数据储存中获取标准气象元素值 listElement = Database().getStandardMeteorologicalElement() To = listElement[0] Po = listElement[1] RHo = listElement[2] lat, lon, Height = coordinationTran.CoordinationTran(ellipsoid).XYZ_to_BLH( approx_position) z = 90 - elevation # 天顶角 P = Po * ((1 - 0.0000266 * Height)**5.225) T = To - 0.0065 * Height + 273.15 e = 6.108 * math.exp((17.15 * T - 4684.0) / (T - 38.45)) * RHo Th = (0.0022768 * P) / (1.0 - 0.00266 * math.cos(2 * lat) - 0.00000028 * Height)\ / math.cos(np.deg2rad(z)) Tw = 0.0022768 * (1255 / T + 0.05) * e / math.cos(np.deg2rad(z)) Tr = Th + Tw return Tr
def calSatelliteAngle(self, center_xyz, x, y, z): sat_xyz = [x, y, z] print(sat_xyz) sat_N, sat_E, sat_H = coordinationTran.CoordinationTran( self.ellipsoid).XYZ_to_NEH(sat_xyz, center_xyz) E = arcsin(sat_H / sqrt(sat_N * sat_N + sat_E * sat_E + sat_H * sat_H)) E = rad2deg(E) # pos_B, pos_L, pos_H = coordinationTran.CoordinationTran(self.ellipsoid).XYZ_to_BLH(center_xyz) # a = 6378137.0 # b = 6356752.314 # e = sqrt((a * a - b * b)/(b * b)) # e_ = sqrt((a * a - b * b)/(a * a)) # o = sqrt(center_xyz[0] * center_xyz[0] + center_xyz[1] * center_xyz[1]) # o1 = arctan2(center_xyz[2] * a, b * o) # B = arctan((center_xyz[2] + e * e * b * sin(o1) * sin(o1) * sin(o1)) / (o - e_ * e_ * a * cos(o1) * cos(o1) * cos(o1))) # X_ = -sin(B) * cos(pos_L) * (x - center_xyz[0]) - sin(B) * sin(pos_L) * (y - center_xyz[1]) + cos(B) * (z - center_xyz[2]) # Y_ = -sin(pos_L) * (x - center_xyz[0]) + cos(pos_L) * (y - center_xyz[1]) # Z_ = fabs(cos(B) * cos(pos_L) * (x - center_xyz[0]) + cos(B) * sin(pos_L) * (y - center_xyz[1]) + sin(B) * (z - center_xyz[2])) # E = arcsin(Z_/sqrt(X_*X_+Y_*Y_+Z_*Z_)) # print(rad2deg(pos_B), rad2deg(pos_L)) # satellite_position = [] # satellite_position.append(x) # satellite_position.append(y) # satellite_position.append(z) # sat_B, sat_L, sat_H = coordinationTran.CoordinationTran(self.ellipsoid).XYZ_to_BLH(satellite_position) # E = rad2deg(arctan((cos(sat_L - pos_L) * cos(pos_B) - 0.15) / sqrt( # 1 - (cos(sat_L - pos_L) * cos(pos_B)) ** 2))) return E
def klobuchar(el, UTCTime, position, satellite, alphaList, betaList, ellipsoid): """ Klobuchar 改正模型 :param el:卫星高度角 :param UT:当地时间UT时 :param alphaList:长度为4 从导航电文获取 :param betaList:长度为4 从导航电文获取 :return: Tg_DOT 电离层时延 """ """ #待确定参数 卫星方位角a 测站P的地心纬度fia_P,地心经度lambda_P """ elli = _ellipsoid(ellipsoid) B_pos, L_pos, H_pos = coordinationTran.CoordinationTran( ellipsoid).XYZ_to_BLH(position) fia_P = np.arctan((1 - elli.e1**2) * np.tan(B_pos)) lambda_P = L_pos B_sat, L_sat, H_sat = XYZ2BLH(satellite[0, 0], satellite[1, 0], satellite[2, 0], elli) a = np.arctan(np.tan(L_sat - L_pos) / np.sin(B_pos)) print("方位角", a) # 测站与P’的地心夹角 单位:度 EA = 445 / (el + 20) - 4 # 交点P'的地心纬度 fia_P_DOT = fia_P + EA * np.cos(a) lambda_P_DOT = lambda_P + EA * np.sin(a) / np.cos(fia_P) # P'的地方时 UT = UTCTime[3] + UTCTime[4] / 60.0 + UTCTime[5] / 3600.0 t = UT + lambda_P_DOT / 15 # 从数据库获取磁北极坐标,计算P'的地磁纬度fia_m fia_earth, lambda_earth = Database.earth_N_pole_coor fia_m = fia_P_DOT + (90 - fia_earth) * np.cos(lambda_P_DOT - lambda_earth) # 计算振幅和周期 A = 0 P = 0 for i in range(4): A += alphaList[i] * (fia_m**i) P += betaList[i] * (fia_m**i) Tg = 5E-9 + A * np.cos(2 * np.pi / P) * (t - 14) secZ = 1 + 2 * (((96 - el) / 90)**3) Tg_DOT = secZ * Tg return Tg_DOT
def tropospheric_delay(approx_position, elevation, epoch, ellipsoid): """ Colins(1999)方法 <p1>对流层延迟 <p2> :param x: 坐标 :param y: :param z: :param elevation: 卫星仰角 :param epoch: 历元 :return: 对流层延迟 单位:m """ lat, lon, ellHeight = coordinationTran.CoordinationTran( ellipsoid).XYZ_to_BLH(approx_position) lat = np.rad2deg(lat) lon = np.rad2deg(lon) ortHeight = ellHeight # Using elliposidal height for now # -------------------- # constants k1 = 77.604 # K/mbar k2 = 382000 # K^2/mbar Rd = 287.054 # J/Kg/K g = 9.80665 # m/s^2 gm = 9.784 # m/s^2 # -------------------- # linear interpolation of meteorological values # Average values ave_params = np.array([[1013.25, 299.65, 26.31, 6.30e-3, 2.77], [1017.25, 294.15, 21.79, 6.05e-3, 3.15], [1015.75, 283.15, 11.66, 5.58e-3, 2.57], [1011.75, 272.15, 6.78, 5.39e-3, 1.81], [1013.00, 263.65, 4.11, 4.53e-3, 1.55]]) # seasonal variations sea_params = np.array([[0.00, 0.00, 0.00, 0.00e-3, 0.00], [-3.75, 7.00, 8.85, 0.25e-3, 0.33], [-2.25, 11.00, 7.24, 0.32e-3, 0.46], [-1.75, 15.00, 5.36, 0.81e-3, 0.74], [-0.50, 14.50, 3.39, 0.62e-3, 0.30]]) # Latitude index Latitude = np.linspace(15, 75, 5) if abs(lat) <= 15.0: indexLat = 0 elif 15 < abs(lat) <= 30: indexLat = 1 elif 30 < abs(lat) <= 45: indexLat = 2 elif 45 < abs(lat) <= 60: indexLat = 3 elif 60 < abs(lat) < 75: indexLat = 4 elif 75 <= abs(lat): indexLat = 5 # ---------------- if indexLat == 0: ave_meteo = ave_params[indexLat, :] sea_meteo = sea_params[indexLat - 1, :] elif indexLat == 5: ave_meteo = ave_params[indexLat - 1, :] sea_meteo = sea_params[indexLat - 1, :] else: ave_meteo = ave_params[indexLat - 1, :] + ( ave_params[indexLat, :] - ave_params[indexLat - 1, :]) * ( abs(lat) - Latitude[indexLat - 1]) / (Latitude[indexLat] - Latitude[indexLat - 1]) sea_meteo = sea_params[indexLat - 1, :] + ( sea_params[indexLat, :] - sea_params[indexLat - 1, :]) * ( abs(lat) - Latitude[indexLat - 1]) / (Latitude[indexLat] - Latitude[indexLat - 1]) # -------------------- doy = datetime2doy(epoch, string=False) if lat >= 0.0: # northern hemisphere doy_min = 28 else: # southern latitudes doy_min = 211 param_meteo = ave_meteo - sea_meteo * np.cos( (2 * np.pi * (doy - doy_min)) / 365.25) pressure, temperature, e, beta, lamda = param_meteo[0], param_meteo[ 1], param_meteo[2], param_meteo[3], param_meteo[4] # -------------------- ave_dry = 1e-6 * k1 * Rd * pressure / gm ave_wet = 1e-6 * k2 * Rd / (gm * (lamda + 1) - beta * Rd) * e / temperature d_dry = ave_dry * (1 - beta * ortHeight / temperature)**(g / Rd / beta) d_wet = ave_wet * (1 - beta * ortHeight / temperature)**(( (lamda + 1) * g / Rd / beta) - 1) m_elev = 1.001 / np.sqrt(0.002001 + np.sin(np.deg2rad(elevation))**2) Vtrop = (d_dry + d_wet) * m_elev return Vtrop
def getStationPosition_PPP(self, sp3PastClass, sp3NowClass, sp3FutureClass, obsClass, obsTime, count_satellite): self._sendInfo("I", "正在读取文件...") # path_sp3FilePast = r"d:\CodeProgram\Python\EMACS\workspace\GNSS\igs20770.sp3" # path_sp3FileNow = r"d:\CodeProgram\Python\EMACS\workspace\GNSS\igs20771.sp3" # path_sp3FileFuture = r"d:\CodeProgram\Python\EMACS\workspace\GNSS\igs20772.sp3" # # path_sp3File = r"d:\CodeProgram\Python\EMACS\workspace\GNSS\igs20771.sp3" # path_oFile = r"D:\CodeProgram\Python\EMACS\workspace\GNSS\GP008301I.19o" # # sp3PastClass = readFile.read_sp3File(path_sp3FilePast) # sp3NowClass = readFile.read_sp3File(path_sp3FileNow) # sp3FutureClass = readFile.read_sp3File(path_sp3FileFuture) # # sp3Class = readFile.read_sp3File(path_sp3File) # obsClass = readFile.read_obsFile(path_oFile) sp3PastEpoch = sp3PastClass.ephemeris.index.values.tolist() sp3NowEpoch = sp3NowClass.ephemeris.index.values.tolist() sp3FutureEpoch = sp3FutureClass.ephemeris.index.values.tolist() # sp3Epoch = sp3Class.ephemeris.index.values.tolist() obsEpoch = obsClass.observation.index.values.tolist() intTimeFormat = lambda strT: list( map(int, ((strT.split())[0]).split("-") + ((strT.split())[1]).split(":"))) # obsTime = obsEpoch[0][0] # print(obsTime) obsTimeList = intTimeFormat(str(obsTime)) time = obsTimeList[3] + obsTimeList[4] / 60 + obsTimeList[5] / 3600 print(time) approx_position = obsClass.approx_position Sat = [] # count_satellite = 6 for k in range(len(obsEpoch)): if obsTime == obsEpoch[k][0]: if (obsEpoch[k][1])[0] == "G": Sat.append(obsEpoch[k][1]) if len(Sat) == count_satellite: break print(Sat) x = [] y = [] z = [] satellite_x = [] satellite_y = [] satellite_z = [] B = [] L = [] self._sendInfo("I", "正在读取卫星数据...") for j in range(len(Sat)): print(Sat[j]) timePastList = [] timeNowList = [] timeFutureList = [] epochPast = [] epochNow = [] epochFuture = [] for i in range(len(sp3PastEpoch)): if sp3PastEpoch[i][1] == Sat[j]: timePastList.append(intTimeFormat(str(sp3PastEpoch[i][0]))) epochPast.append(sp3PastEpoch[i][0]) for i in range(len(sp3NowEpoch)): if sp3NowEpoch[i][1] == Sat[j]: timeNowList.append(intTimeFormat(str(sp3NowEpoch[i][0]))) epochNow.append(sp3NowEpoch[i][0]) for i in range(len(sp3FutureEpoch)): if sp3FutureEpoch[i][1] == Sat[j]: timeFutureList.append( intTimeFormat(str(sp3FutureEpoch[i][0]))) epochFuture.append(sp3FutureEpoch[i][0]) # print(epochPast) timePastSeconds = [] timeNowSeconds = [] timeFutureSeconds = [] dx = [] dy = [] dz = [] for i in range(len(timePastList)): timePastSeconds.append(timePastList[i][3] + timePastList[i][4] / 60 + timePastList[i][5] / 3600) dx.append( (sp3PastClass.ephemeris.loc[(epochPast[i], Sat[j])]["X"]) * 1000) dy.append( (sp3PastClass.ephemeris.loc[(epochPast[i], Sat[j])]["Y"]) * 1000) dz.append( (sp3PastClass.ephemeris.loc[(epochPast[i], Sat[j])]["Z"]) * 1000) for i in range(len(timeNowList)): timeNowSeconds.append(timeNowList[i][3] + timeNowList[i][4] / 60 + timeNowList[i][5] / 3600) dx.append( (sp3NowClass.ephemeris.loc[(epochNow[i], Sat[j])]["X"]) * 1000) dy.append( (sp3NowClass.ephemeris.loc[(epochNow[i], Sat[j])]["Y"]) * 1000) dz.append( (sp3NowClass.ephemeris.loc[(epochNow[i], Sat[j])]["Z"]) * 1000) for i in range(len(timeFutureList)): timeFutureSeconds.append(timeFutureList[i][3] + timeFutureList[i][4] / 60 + timeFutureList[i][5] / 3600) dx.append((sp3FutureClass.ephemeris.loc[(epochFuture[i], Sat[j])]["X"]) * 1000) dy.append((sp3FutureClass.ephemeris.loc[(epochFuture[i], Sat[j])]["Y"]) * 1000) dz.append((sp3FutureClass.ephemeris.loc[(epochFuture[i], Sat[j])]["Z"]) * 1000) x.append(dx) y.append(dy) z.append(dz) timeSeconds = [] for i in range(len(timePastSeconds)): timeSeconds.append(timePastSeconds[i] - 24) for i in range(len(timeNowSeconds)): timeSeconds.append(timeNowSeconds[i]) for i in range(len(timeFutureSeconds)): timeSeconds.append(timeFutureSeconds[i] + 24) px = 0 py = 0 pz = 0 for m in range(len(timeSeconds)): px_delta = x[j][m] * self.coefficients(time, timeSeconds[m], timeSeconds) px += px_delta py_delta = y[j][m] * self.coefficients(time, timeSeconds[m], timeSeconds) py += py_delta pz_delta = z[j][m] * self.coefficients(time, timeSeconds[m], timeSeconds) pz += pz_delta satellite_x.append(px) satellite_y.append(py) satellite_z.append(pz) satelliteName = Sat[j] waveBand = "C1C" waveDistance = obsClass.observation.loc[(obsTime, satelliteName)][waveBand] approxDistance = np.sqrt((satellite_x[j] - approx_position[0])**2 + (satellite_y[j] - approx_position[1])**2 + (satellite_z[j] - approx_position[2])**2) # TODO 查找卫星仰角 satelliteAngle = self.calSatelliteAngle(approx_position, px, py, pz) self._sendInfo("K", " -{} 卫星仰角:{}".format(Sat[j], satelliteAngle)) # 对流层延迟/电离层延迟改正 Vion = 0 Vtrop = tropCorrection.standardMeteorologicalMethod( approx_position, self.ellipsoid, satelliteAngle) B.append([ -(satellite_x[j] - approx_position[0]) / approxDistance, -(satellite_y[j] - approx_position[1]) / approxDistance, -(satellite_z[j] - approx_position[2]) / approxDistance, -1 ]) L.append([approxDistance + Vion + Vtrop - approxDistance]) self._sendInfo("I", "正在进行平差求解...") # 平差求解 matrix_B = np.mat(B) matrix_L = np.mat(L) self._sendInfo("K", "--matrix_B:\n{}\n".format(matrix_B)) self._sendInfo("K", "--matrix_L:\n{}\n".format(matrix_L.tolist())) Q = np.linalg.inv(np.transpose(matrix_B) * matrix_B) matrix_x = Q * (np.transpose(matrix_B) * matrix_L) matrix_v = matrix_B * matrix_x - matrix_L self._sendInfo("K", "--matrix_x:\n{}\n".format(matrix_x.tolist())) self._sendInfo("K", "--matrix_v:\n{}\n".format(matrix_v.tolist())) sigma_o = np.sqrt( (np.transpose(matrix_v) * matrix_v)[0, 0] / (count_satellite - 4)) self._sendInfo("K", "中误差:{} mm".format(sigma_o * 1000)) stationPosition = [ approx_position[0] + matrix_x[0, 0], approx_position[1] + matrix_x[1, 0], approx_position[2] + matrix_x[2, 0] ] self._sendInfo( "K", "近似坐标:\n{} \n最后坐标:\n{}".format(approx_position, stationPosition)) self._sendInfo("测站", obsClass.stationName) self._sendInfo("X/m", str(approx_position[0])) self._sendInfo("Y/m", str(approx_position[1])) self._sendInfo("Z/m", str(approx_position[2])) coor_B, coor_L, coor_H = coordinationTran.CoordinationTran( self.ellipsoid).XYZ_to_BLH(stationPosition) self._sendInfo("B/°", str(np.rad2deg(coor_B))) self._sendInfo("L/°", str(np.rad2deg(coor_L))) self._sendInfo("H/m", str(coor_H)) # 调用百度地图API获取经纬度对应的地理位置信息 pointInfomation = baiduMap.getAddressInfo(np.rad2deg(coor_L), np.rad2deg(coor_B)) # print("Poi0",pointInfomation) self._sendInfo("地理信息", pointInfomation) # 精度评价: GDOP / PDOP / TDOP / HDOP / VDOP # 三维点位精度衰减因子 mo = sigma_o * sigma_o PDOP = np.sqrt(Q[0, 0] + Q[1, 1] + Q[2, 2]) mP = mo * PDOP # 时间精度衰减因子 TDOP = np.sqrt(Q[3, 3]) mT = mo * TDOP # 几何精度衰减因子 GDOP = np.sqrt(Q[0, 0] + Q[1, 1] + Q[2, 2] + Q[3, 3]) mG = mo * GDOP # 坐标系统转换为BLH的系数矩阵 matrix_BLH_B = np.mat([[ -np.sin(coor_B) * np.cos(coor_L), -np.sin(coor_B) * np.sin(coor_L), np.cos(coor_B) ], [-np.sin(coor_L), np.cos(coor_L), 0], [ np.cos(coor_B) * np.cos(coor_L), np.cos(coor_B) * np.cos(coor_L), np.sin(coor_B) ]]) Q_dot = matrix_BLH_B * Q[0:3, 0:3] * np.transpose(matrix_BLH_B) HDOP = np.sqrt(Q_dot[0, 0] + Q_dot[1, 1]) mH = mo * HDOP VDOP = np.sqrt(Q_dot[2, 2]) mV = mo * VDOP asd = [PDOP, mP, TDOP, mT, GDOP, mG, HDOP, mH, VDOP, mV] asdName = [ "PDOP/m", "mP/m", "TDOP/m", "mT/m", "GDOP/m", "mG/m", "HDOP/m", "mH/m", "VDOP/m", "mV/m" ] for g in range(len(asd)): self._sendInfo(asdName[g], str(asd[g])) self.resDict["pointID"].append("nan") self.resDict["X"].append(stationPosition[0]) self.resDict["Y"].append(stationPosition[1]) self.resDict["Z"].append(stationPosition[2]) self.resDict["B"].append(np.rad2deg(coor_B)) self.resDict["L"].append(np.rad2deg(coor_L)) self.resDict["H"].append(coor_H) self.resDict["PDOP"].append(PDOP) self.resDict["mP"].append(mP) self.resDict["TDOP"].append(TDOP) self.resDict["mT"].append(mT) self.resDict["GDOP"].append(GDOP) self.resDict["mG"].append(mG) self.resDict["VDOP"].append(VDOP) self.resDict["mV"].append(mV) self.resDict["HDOP"].append(HDOP) self.resDict["mH"].append(mH) self.resDict["information"].append(pointInfomation)
def getStationPosition_SPP(self, observationEpoch, waveBandOne, waveBandSec, Sat, count_satellite, obsClass, navClass): # print(coordinationTran.CoordinationTran(self.ellipsoid).XYZ_to_BLH( # [6378020.461599736, 12739.801484877651, 49091.74122939511])) # 卫星数量 # count_satellite = len(PRN) # 光速 m/s c = 299792458 # 地球自转角速度rad/s -RotationalAngularVelocity earth_RAV = 7.29211511467e-5 # 测站近似坐标,一维list Position = obsClass.approx_position stationPosition = obsClass.approx_position # 判断与观测时间最近的导航电文时间 delta = [] navEpoch = navClass.navigation.epoch.index.values.tolist() for i in range(len(navEpoch)): delta.append((navEpoch[i][0] - observationEpoch).seconds) delta = list(map(abs, delta)) num = 0 navTime = "" for i in range(len(delta)): mindelta = min(delta) navTime = observationEpoch + datetime.timedelta(seconds=mindelta) if navTime.minute != 0 and navTime.second != 0: num = delta.index(mindelta) Sat.pop(num) delta.pop(num) else: break # print(delta.pop(num)) # print(navTime) PRN = [] for i in range(len(Sat)): PRN.append(Sat[i]) if len(PRN) >= count_satellite: break # print(PRN) # 构建系数矩阵 B = [] L = [] # 设置测站 self._sendInfo("测站", obsClass.stationName) cVtr = 0 VX = 1 VY = 1 VZ = 1 while abs(VX) > 1e-4 or abs(VY) > 1e-4 or abs(VZ) > 1e-4: approxPosition = stationPosition B = [] L = [] for i in range(len(PRN)): satelliteName = PRN[i] print(satelliteName) print(navTime) # 根据行键获取数据,key = ('2019-10-28 10:00:00', 'G10'),使用iloc可以根据行序获取,键值从观测文件找 # navEpochData = navClass.navigation.loc[(navClass.navigation.index.values[i][0], satelliteName)].tolist() navEpochData = navClass.navigation.loc[( navTime, satelliteName)].tolist() # 根据接收时间和伪距,计算信号发射时刻 # time_receiveSignal = "2019-10-28 08:33:15" waveDistanceOne = obsClass.observation.loc[( observationEpoch, satelliteName)][waveBandOne] waveDistanceSec = obsClass.observation.loc[( observationEpoch, satelliteName)][waveBandSec] # intTimeFormat 实现如下转换:2019-10-28 08:33:15 --> [2019, 10, 28, 8, 33, 15] intTimeFormat = lambda strT: list( map(int, ((strT.split())[0]).split("-") + ((strT.split())[1]).split(":"))) UTCTimeList = intTimeFormat(str(observationEpoch)) print(UTCTimeList) time_rec = TimeSystemChange(UTCTimeList[0], UTCTimeList[1], UTCTimeList[2], UTCTimeList[3], UTCTimeList[4], UTCTimeList[5]) # week, tow = time_rec.UTC2GPSTime() tow = 5 * 86400 + UTCTimeList[3] * 3600 + UTCTimeList[ 4] * 60 + UTCTimeList[5] # print(Tow) approxDistance = waveDistanceOne temp = 0 Vts = 0 xyz = [] while abs(temp - approxDistance) > 1e-6: temp = approxDistance teta_ts = approxDistance / c time_sendSignal = tow - teta_ts Vts, xyz = satelliteOrbetEtc.getSatellitePositon_II( time_sendSignal, navEpochData) # 对卫星坐标进行地球自转改正 xyz = mat([[0, sin(earth_RAV * teta_ts), 0], [-sin(earth_RAV * teta_ts), 0, 0], [0, 0, 0] ]) * mat(xyz) + mat(xyz) # print(PRN[i]+"卫星位置:", xyz) # xyz = xyz.tolist() # 计算近似站星距离/伪距 sum = 0 for k in range(3): sum += (xyz[k, 0] - approxPosition[k]) * ( xyz[k, 0] - approxPosition[k]) approxDistance = sqrt(sum) # 卫星仰角 satelliteAngle = self.calSatelliteAngle( approxPosition, xyz[0, 0], xyz[1, 0], xyz[2, 0]) if satelliteAngle < 7.0: continue else: # 对流层延迟/电离层延迟改正 # 无电离层的双频距离改正 waveDistance = 2.54573 * waveDistanceOne - 1.54573 * waveDistanceSec # 对流层改正 Vtrop = tropCorrection.standardMeteorologicalMethod( approxPosition, self.ellipsoid, satelliteAngle) B.append([ -(xyz[0, 0] - approxPosition[0]) / approxDistance, -(xyz[1, 0] - approxPosition[1]) / approxDistance, -(xyz[2, 0] - approxPosition[2]) / approxDistance, 1 ]) L.append([ waveDistance + c * Vts - cVtr - Vtrop - approxDistance ]) print("==+==", PRN[i], waveDistance - approxDistance) # 循环解算系数等完成 # 平差求解 matrix_B = mat(B) matrix_L = mat(L) Q = linalg.inv(transpose(matrix_B) * matrix_B) matrix_x = Q * (transpose(matrix_B) * matrix_L) matrix_v = matrix_B * matrix_x - matrix_L sigma_o = sqrt( (transpose(matrix_v) * matrix_v)[0, 0] / (count_satellite - 4)) # 三维点位精度衰减因子 mo = sigma_o * sigma_o PDOP = sqrt(Q[0, 0] + Q[1, 1] + Q[2, 2]) mP = mo * PDOP # 时间精度衰减因子 TDOP = sqrt(Q[3, 3]) mT = mo * TDOP # 几何精度衰减因子 GDOP = sqrt(Q[0, 0] + Q[1, 1] + Q[2, 2] + Q[3, 3]) mG = mo * GDOP self._sendInfo("K", "--matrix_B") self.outputFormatList("K", matrix_B.tolist(), 13) # self._sendInfo("K", "--matrix_B:{}\n".format(matrix_B.tolist())) self._sendInfo("K", "--matrix_L:{}\n".format(matrix_L.tolist())) self._sendInfo("K", "--matrix_x:{}\n".format(matrix_x.tolist())) self._sendInfo("K", "--matrix_v:{}\n".format(matrix_v.tolist())) self._sendInfo("K", "中误差:{} mm".format(sigma_o * 1000)) # 改正 stationPosition = [ approxPosition[0] + matrix_x[0, 0], approxPosition[1] + matrix_x[1, 0], approxPosition[2] + matrix_x[2, 0] ] VX = matrix_x[0, 0] VY = matrix_x[1, 0] VZ = matrix_x[2, 0] cVtr = cVtr + matrix_x[3, 0] self._sendInfo( "K", "近似坐标:{} \n最后坐标:{}\n-----------------\n".format( Position, stationPosition)) self._sendInfo("坐标X/m", str(stationPosition[0])) self._sendInfo("坐标Y/m", str(stationPosition[1])) self._sendInfo("坐标Z/m", str(stationPosition[2])) coor_B, coor_L, coor_H = coordinationTran.CoordinationTran( self.ellipsoid).XYZ_to_BLH(stationPosition) self._sendInfo("B/°", str(rad2deg(coor_B))) self._sendInfo("L/°", str(rad2deg(coor_L))) self._sendInfo("H/m", str(coor_H)) # 调用百度地图API获取经纬度对应的地理位置信息 pointInfomation = baiduMap.getAddressInfo(rad2deg(coor_L), rad2deg(coor_B)) # print("Poi0",pointInfomation) self._sendInfo("地理信息", pointInfomation) # 精度评价: GDOP / PDOP / TDOP / HDOP / VDOP # 坐标系统转换为BLH的系数矩阵 matrix_BLH_B = mat([[ -sin(coor_B) * cos(coor_L), -sin(coor_B) * sin(coor_L), cos(coor_B) ], [-sin(coor_L), cos(coor_L), 0], [ cos(coor_B) * cos(coor_L), cos(coor_B) * cos(coor_L), sin(coor_B) ]]) Q_dot = matrix_BLH_B * Q[0:3, 0:3] * transpose(matrix_BLH_B) HDOP = sqrt(Q_dot[0, 0] + Q_dot[1, 1]) mH = mo * HDOP VDOP = sqrt(Q_dot[2, 2]) mV = mo * VDOP asd = [PDOP, mP, TDOP, mT, GDOP, mG, HDOP, mH, VDOP, mV] asdName = [ "PDOP/m", "mP/m", "TDOP/m", "mT/m", "GDOP/m", "mG/m", "HDOP/m", "mH/m", "VDOP/m", "mV/m" ] for g in range(len(asd)): self._sendInfo(asdName[g], str(round(asd[g], 5))) self.resDict["pointID"].append(obsClass.stationName) self.resDict["X"].append(stationPosition[0]) self.resDict["Y"].append(stationPosition[1]) self.resDict["Z"].append(stationPosition[2]) self.resDict["B"].append(rad2deg(coor_B)) self.resDict["L"].append(rad2deg(coor_L)) self.resDict["H"].append(coor_H) self.resDict["PDOP"].append(PDOP) self.resDict["mP"].append(mP) self.resDict["TDOP"].append(TDOP) self.resDict["mT"].append(mT) self.resDict["GDOP"].append(GDOP) self.resDict["mG"].append(mG) self.resDict["VDOP"].append(VDOP) self.resDict["mV"].append(mV) self.resDict["HDOP"].append(HDOP) self.resDict["mH"].append(mH) self.resDict["information"].append(pointInfomation)