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)