KJ135
/
microproduct-l-sar
mirror of http://172.16.0.12:5000/WBJY/microproduct-l-sar.git
4
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
microproduct-l-sar/vegetationHeight-L-SAR/SatelliteGPS.py

819 lines
34 KiB
Python
Raw Permalink Normal View History

上传L-SAR代码
2024-01-03 01:42:21 +00:00
import copy
import json
import math
import datetime
from xml.etree.ElementTree import ElementTree
from collections import namedtuple
import numpy as np
import numbers
# cosine in degress (math could be <a href="http://numpy.scipy.org/">numpy</a>
import xmltodict
cosd = lambda x: np.cos(np.radians(x))
# sine in degrees
sind = lambda x: np.sin(np.radians(x))
# tangent, in degrees
tand = lambda x: np.tan(np.radians(x))
# arc tan in degrees (2 arg)
arctand2 = lambda y, x: np.degrees(np.arctan2(y, x))
# arc tan in degrees (1 arg)
arctand = lambda x: np.degrees(np.arctan(x))
reference_ellipsoid_params = {
'a': 6378137, # 椭球长半轴 (m)
'b': 6356752, # 椭球短半轴 (m)
# 其他椭球参数...
}
LIGHTSPEED = 299792458
# 第一偏心率的平方
e2 = (reference_ellipsoid_params['a']**2 - reference_ellipsoid_params['b']**2) / reference_ellipsoid_params['a']**2
class SatelliteOrbit(object):
def __init__(self) -> None:
super().__init__()
self.starttime = 1262275200.0
self.modelName=""
def get_starttime(self):
'''
返回卫星轨道时间起算点
'''
return self.starttime
def ReconstructionSatelliteOrbit(self, GPSPoints_list):
'''
重建卫星轨道，使用多项式拟合法
args:
GPSPoints_list:GPS 卫星轨道点
return：
SatelliteOrbitModel 卫星轨道模型
'''
self.SatelliteOrbitModel = None
def SatelliteSpaceState(self, time_float):
'''
根据时间戳，返回对应时间的卫星的轨迹状态
args:
time_float:时间戳
return：
State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
'''
return None
class SatelliteOrbitFitPoly(SatelliteOrbit):
'''
继承于SatelliteOribit类，为拟合多项式实现方法
'''
def __init__(self) -> None:
super().__init__()
self.modelName = "多项式"
self.polynum = 4
def ReconstructionSatelliteOrbit(self, GPSPoints_list, starttime):
if len(GPSPoints_list) == 2:
self.polynum = 1
self.starttime = starttime
record_count = len(GPSPoints_list)
time_arr = np.zeros((record_count, 1), dtype=np.float64) # 使用np.float64只是为了精度高些；如果32位也能满足需求，请用32位
state_arr = np.zeros((record_count, 6), dtype=np.float64)
A_arr = np.zeros((self.polynum + 1, 6), dtype=np.float64) # 四次项
X = np.ones((record_count, self.polynum + 1), dtype=np.float64) # 记录时间坐标
# 将点记录转换为自变量矩阵、因变量矩阵
for i in range(record_count):
GPSPoint = GPSPoints_list[i]
time_ = GPSPoint[0] - self.starttime # 为了保证精度，对时间进行缩放
X[i, :] = np.array([1, time_])
state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6) # 空间坐标
self.model_f = []
for i in range(6):
Y = state_arr[:, i].reshape(-1, 1)
A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]
self.A_arr = copy.deepcopy(A_arr.copy())
return self.A_arr
elif len(GPSPoints_list) > 6:
self.polynum = 4
# 多项式的节点数，理论上是超过5个可以起算，这里为了精度选择10个点起算。
# 多项式 XA=Y ==> A=(X'X)^X'Y，其中 A 为待求系数，X为变量，Y为因变量
# 这里使用三次项多项式，共有6组参数。
# 声明自变量，因变量，系数矩阵
self.starttime = starttime
record_count = len(GPSPoints_list)
time_arr = np.zeros((record_count, 1), dtype=np.float64) # 使用np.float64只是为了精度高些；如果32位也能满足需求，请用32位
state_arr = np.zeros((record_count, 6), dtype=np.float64)
A_arr = np.zeros((self.polynum + 1, 6), dtype=np.float64) # 四次项
X = np.ones((record_count, self.polynum + 1), dtype=np.float64) # 记录时间坐标
# 将点记录转换为自变量矩阵、因变量矩阵
for i in range(record_count):
GPSPoint = GPSPoints_list[i]
time_ = GPSPoint[0] - self.starttime # 为了保证精度，对时间进行缩放
X[i, :] = np.array([1, time_, time_ ** 2, time_ ** 3, time_ ** 4])
state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6) # 空间坐标
self.model_f = []
for i in range(6):
Y = state_arr[:, i].reshape(-1, 1)
A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]
self.A_arr = copy.deepcopy(A_arr.copy())
''' 测试误差
from matplotlib import pyplot
label_list=['x','y','z','vx','vy','vz']
color_list=['r','g','b','gold','gray','pink']
pyplot.figure()
for i in range(6):
Y = state_arr[:, i]
Y_predict=self.model_f[i](X)
pyplot.subplot(int("23{}".format(i+1)))
d=Y-Y_predict
pyplot.plot(X,d,label=label_list[i],color=color_list[i])
pyplot.title("max:{}".format(np.max(d)))
#self.model_f.append(interpolate.interp1d(X,Y,kind='cubic',fill_value='extrapolate'))
pyplot.legend()
pyplot.show()
'''
return self.A_arr
else:
self.A_arr = None
return None
def SatelliteSpaceState(self, time_float):
'''
逐像素求解
根据时间戳，返回对应时间的卫星的轨迹状态，会自动计算与起算时间之差
args:
time_float:时间戳
return：
State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
'''
if self.model_f is None:
return None
result_arr = np.zeros((1, 7))
time_float = time_float - self.starttime
result_arr[0, 0] = time_float
# time_arr[0, 4] = time_arr[0, 3] * time_float ** 4
time_float = np.array([1, time_float, time_float ** 2, time_float ** 3, time_float ** 4]).reshape(1, 5)
result_arr = np.matmul(time_float, self.A_arr)
return [time_float, result_arr]
def getSatelliteSpaceState(self, time):
'''
矩阵求解
根据时间戳矩阵，返回对应时刻的卫星空间状态（位置，速度），且会自动计算与起算时间之差
args:
time_array:nparray nx1 时间戳
return:
SatellitSpaceStateArray:nparray nx6 状态信息
'''
time_float = time.timestamp()
time_float = time_float - self.starttime
#
px = 0
py = 0
pz = 0
vx = 0
vy = 0
vz = 0
for ii in range(self.polynum):
px += self.A_arr[ii][0] * time_float ** ii
py += self.A_arr[ii][1] * time_float ** ii
pz += self.A_arr[ii][2] * time_float ** ii
vx += self.A_arr[ii][3] * time_float ** ii
vy += self.A_arr[ii][4] * time_float ** ii
vz += self.A_arr[ii][5] * time_float ** ii
sv = [px, py, pz, vx, vy, vz]
return sv
# if self.model_f is None:
# return None # 返回None,表示没有结果
# if self.polynum == 4:
# n = time_array.shape[0]
# result_arr_ = np.zeros((n, 6), dtype=np.float64)
# time_float = time_array - self.starttime
# time_float = time_float.reshape(-1) # nx1
# time_arr = np.ones((time_float.shape[0], 5)) # nx5
# time_arr[:, 1] = time_float
# time_arr[:, 2] = time_float ** 2
# time_arr[:, 3] = time_float ** 3
# time_arr[:, 4] = time_float ** 4
# result_arr_ = np.matmul(time_arr, self.A_arr) # nx5 5x6
# # time_arr[0, 4] = time_arr[0, 3] * time_float ** 4
# # result_arr=result_arr_
# return result_arr_ # 位置矩阵
# else:
# n = time_array.shape[0]
# result_arr_ = np.zeros((n, 6), dtype=np.float64)
# time_float = time_array - self.starttime
# time_float = time_float.reshape(-1) # nx1
# time_arr = np.ones((time_float.shape[0], self.polynum + 1)) # nx5
# time_arr[:, 1] = time_float
# result_arr_ = np.matmul(time_arr, self.A_arr) # nx5 5x6
# # time_arr[0, 4] = time_arr[0, 3] * time_float ** 4
# # result_arr=result_arr_
# return result_arr_ # 位置矩阵
class SARgps:
@staticmethod
def xyz_to_llh(xyz):
"""xyz_to_llh(xyz): returns llh=(lat (deg), lon (deg), h (meters)) for the instance ellipsoid \n
given the coordinates of a point at xyz=(z,y,z) (meters). \n
Based on closed form solution of H. Vermeille, Journal of Geodesy (2002) 76:451-454. \n
Handles simple list or tuples (xyz represents a single point) or a list of lists or tuples (xyz represents several points)"""
a2 = reference_ellipsoid_params['a'] ** 2
e4 = e2 ** 2
# just to cast back to single list once done
onePosition = False
if isinstance(xyz[0], numbers.Real):
xyz = [xyz]
onePosition = True
r_llh = [0] * 3
d_llh = [[0] * 3 for i in range(len(xyz))]
for i in range(len(xyz)):
xy2 = xyz[i][0] ** 2 + xyz[i][1] ** 2
p = (xy2) / a2
q = (1. - e2) * xyz[i][2] ** 2 / a2
r = (p + q - e4) / 6.
s = e4 * p * q / (4. * r ** 3)
t = (1. + s + math.sqrt(s * (2. + s))) ** (1. / 3.)
u = r * (1. + t + 1. / t)
v = math.sqrt(u ** 2 + e4 * q)
w = e2 * (u + v - q) / (2. * v)
k = math.sqrt(u + v + w ** 2) - w
D = k * math.sqrt(xy2) / (k + e2)
r_llh[0] = math.atan2(xyz[i][2], D)
r_llh[1] = math.atan2(xyz[i][1], xyz[i][0])
r_llh[2] = (k + e2 - 1.) * math.sqrt(D ** 2 + xyz[i][2] ** 2) / k
d_llh[i][0] = math.degrees(r_llh[0])
d_llh[i][1] = math.degrees(r_llh[1])
d_llh[i][2] = r_llh[2]
if onePosition:
return d_llh[0]
else:
return d_llh
@staticmethod
def llh_to_xyz(llh):
"""llh_to_xyz(llh): returns (z,y,z) (meters) coordinates of a point given the point at \nllh=(lat (deg), lon (deg), h (meters)) for the instance ellipsoid\n
Handles simple list or tuples (xyz represents a single point) or a list of lists or tuples (xyz represents several points)
"""
# just to cast back to single list once done
onePosition = False
if isinstance(llh[0], numbers.Real):
llh = [llh]
onePosition = True
r_v = [[0] * 3 for i in range(len(llh))]
for i in range(len(llh)):
r_lat = math.radians(llh[i][0])
r_lon = math.radians(llh[i][1])
hgt = llh[i][2]
r_re = reference_ellipsoid_params['a'] / math.sqrt(1.0 - e2 * math.sin(r_lat) ** 2)
r_v[i][0] = (r_re + hgt) * math.cos(r_lat) * math.cos(r_lon)
r_v[i][1] = (r_re + hgt) * math.cos(r_lat) * math.sin(r_lon)
r_v[i][2] = (r_re * (1.0 - e2) + hgt) * math.sin(r_lat)
if onePosition:
return r_v[0]
else:
return r_v
def convertToDateTime(self, string):
dt = datetime.datetime.strptime(string, "%Y-%m-%dT%H:%M:%S.%f")
return dt
def normal_radius_of_curvature(self, lat):
"""East Radius (eastRad): Normal radius of curvature (N), meters for
latitude in degrees """
return (
reference_ellipsoid_params['a'] ** 2 /
((reference_ellipsoid_params['a'] * cosd(lat)) ** 2 + (reference_ellipsoid_params['b'] * sind(lat)) ** 2) ** 0.5
)
def LatLonHgt2XYZ(self, lat, lon, h):
"""LatLonHgt2XYZ(lat, lon, h) --> (x, y, z)
lat is the latitude (deg)
lon is the longitude (deg)
h is the heigh (m)
(x, y, z) is a tuple of ECEF coordinates (m)
"""
N = self.normal_radius_of_curvature(lat)
cos_lat = cosd(lat)
X = cos_lat * cosd(lon) * (N + h)
Y = cos_lat * sind(lon) * (N + h)
Z = sind(lat) * ((1 - e2) * N + h)
return (X, Y, Z)
def FindInfomationFromJson(self, HeaderFile_dom_json, node_path_list):
"""
在Json文件中，按照指定路径解析出制定节点
"""
result_node = HeaderFile_dom_json
for nodename in node_path_list:
result_node = result_node[nodename]
return result_node
def ReconstructionSatelliteOrbit(self, GPSPoints_list, starttime):
if len(GPSPoints_list) == 2:
self.polynum = 1
self.starttime = starttime
record_count = len(GPSPoints_list)
time_arr = np.zeros((record_count, 1), dtype=np.float64) # 使用np.float64只是为了精度高些；如果32位也能满足需求，请用32位
state_arr = np.zeros((record_count, 6), dtype=np.float64)
A_arr = np.zeros((self.polynum + 1, 6), dtype=np.float64) # 四次项
X = np.ones((record_count, self.polynum + 1), dtype=np.float64) # 记录时间坐标
# 将点记录转换为自变量矩阵、因变量矩阵
for i in range(record_count):
GPSPoint = GPSPoints_list[i]
time_ = GPSPoint[0] - self.starttime # 为了保证精度，对时间进行缩放
X[i, :] = np.array([1, time_])
state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6) # 空间坐标
self.model_f = []
for i in range(6):
Y = state_arr[:, i].reshape(-1, 1)
A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]
self.A_arr = copy.deepcopy(A_arr.copy())
return self.A_arr
elif len(GPSPoints_list) > 6:
# 多项式的节点数，理论上是超过5个可以起算，这里为了精度选择10个点起算。
# 多项式 XA=Y ==> A=(X'X)^X'Y，其中 A 为待求系数，X为变量，Y为因变量
# 这里使用三次项多项式，共有6组参数。
# 声明自变量，因变量，系数矩阵
self.starttime = starttime
record_count = len(GPSPoints_list)
time_arr = np.zeros((record_count, 1), dtype=np.float64) # 使用np.float64只是为了精度高些；如果32位也能满足需求，请用32位
state_arr = np.zeros((record_count, 6), dtype=np.float64)
A_arr = np.zeros((self.polynum, 6), dtype=np.float64) # 四次项
X = np.ones((record_count, self.polynum), dtype=np.float64) # 记录时间坐标
# 将点记录转换为自变量矩阵、因变量矩阵
for i in range(record_count):
GPSPoint = GPSPoints_list[i]
time_ = GPSPoint[0] - self.starttime # 为了保证精度，对时间进行缩放
X[i, :] = np.array(list(map(lambda ii: time_ ** ii, range(self.polynum))))
state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6) # 空间坐标
self.model_f = []
for i in range(6):
Y = state_arr[:, i].reshape(-1, 1)
A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]
self.A_arr = A_arr.copy()
return self.A_arr
else:
self.A_arr = None
return None
def getGPSModel(self, xml_path):
GPSLists = []
root = ElementTree(file=xml_path).getroot()
with open(xml_path, 'r', encoding='utf-8') as fp:
HeaderFile_dom_str = fp.read()
HeaderFile_dom = xmltodict.parse(HeaderFile_dom_str) # 将XML转成json文本
HeaderFile_dom_json = json.loads(json.dumps(HeaderFile_dom)) # 将json字符串，转成json对象（对应于python中的dict）
GPSNode = ['level1Product','platform','orbit', 'stateVec']
GPSNodeNames = ['timeUTC', 'posX', 'posY', 'posZ', 'velX', 'velY', 'velZ']
GPSTime = "%Y-%m-%dT%H:%M:%S.%f"
GPSPoints = self.FindInfomationFromJson(HeaderFile_dom_json, GPSNode)
dataStartTime = self.convertToDateTime(
root.find("productInfo").find("sceneInfo").find("start").find("timeUTC").text).timestamp()
dataStopTime = self.convertToDateTime(
root.find("productInfo").find("sceneInfo").find("stop").find("timeUTC").text).timestamp()
centerTime = (dataStopTime - dataStartTime) / 2 + dataStartTime
for GPSPoint in GPSPoints:
GPSPoint = [
datetime.datetime.strptime(GPSPoint[GPSNodeNames[0]], GPSTime).timestamp(), # TimeStamp
float(GPSPoint[GPSNodeNames[1]]), # Xp
float(GPSPoint[GPSNodeNames[2]]), # Yp
float(GPSPoint[GPSNodeNames[3]]), # Zp
float(GPSPoint[GPSNodeNames[4]]), # Vx
float(GPSPoint[GPSNodeNames[5]]), # Vy
float(GPSPoint[GPSNodeNames[6]])] # VZ
GPSLists.append(GPSPoint)
SatelliteOrbitModel = SatelliteOrbitFitPoly()
if SatelliteOrbitModel.ReconstructionSatelliteOrbit(GPSLists, centerTime) is None:
return None
return SatelliteOrbitModel
def calBaseline(self, slantRange_m, azimuthTime_m, azimuthTime_a, SatelliteOrbit_m, SatelliteOrbit_a, wvl, mas_xml, aux_xml):
start = datetime.datetime.now()
aux_referenceTime = self.getReferenceTime(aux_xml)
Bpar = np.zeros((len(azimuthTime_m), len(slantRange_m)), dtype=np.float32)
Bperp = np.zeros((len(azimuthTime_m), len(slantRange_m)), dtype=np.float32)
for ii, taz in enumerate(azimuthTime_m):
referenceSV = SatelliteOrbit_m.getSatelliteSpaceState(taz)
mxyz = np.array(referenceSV[0:3])
mvel = np.array(referenceSV[3:6])
for jj, rng in enumerate(slantRange_m):
# start = datetime.datetime.now()
llh = self.rdr2geo(referenceSV, taz, rng, wvl, SatelliteOrbit_m, azimuthTime_m, slantRange_m, mas_xml)
tarxyz_temp = self.LatLonHgt2XYZ(llh[0], llh[1], llh[2])
targxyz = np.array((tarxyz_temp[0], tarxyz_temp[1], tarxyz_temp[2]))
slvTime, slvrng = self.geo2rdr(llh, azimuthTime_a, SatelliteOrbit_a, wvl, aux_referenceTime)
# end = datetime.datetime.now()
# deffT = (end - start).total_seconds()
# print('消耗时间为： ' + str(deffT))
secondarySV = SatelliteOrbit_a.getSatelliteSpaceState(slvTime)
sxyz = np.array(secondarySV[0:3])
aa = np.linalg.norm(sxyz - mxyz)
costheta = (rng * rng + aa * aa - slvrng * slvrng) / (2. * rng * aa)
Bpar[ii, jj] = aa * costheta
perp = aa * np.sqrt(1 - costheta * costheta)
direction = np.sign(np.dot(np.cross(targxyz - mxyz, sxyz - mxyz), mvel))
Bperp[ii, jj] = direction * perp
end = datetime.datetime.now()
deffT = (end - start).total_seconds()
print('消耗时间为： ' + str(deffT))
return Bpar, Bperp
def rdr2geo(self, sv, taz, rng, wvl, SatelliteOrbit_m, azimuthTime, slantRange, mas_xml, height=0., side=-1):
hdg = self.getENUHeading(SatelliteOrbit_m, taz)
# sv = SatelliteOrbit_m.getSatelliteSpaceState(taz)
pos = sv[0:3]
vel = sv[3:6]
# vmag = np.linalg.norm(vel)
dopfact = 0.0
# dopfact = self.getDoppler(taz, rng, azimuthTime, slantRange, mas_xml) * 0.5 * wvl * rng / vmag
satLLH = self.xyz_to_llh(pos)
self.setSCH(satLLH[0], satLLH[1], hdg)
radius = self.pegRadCur
satVec = np.array(pos)
velVec = np.array(vel)
###Setup TCN basis
clat = np.cos(np.radians(satLLH[0]))
slat = np.sin(np.radians(satLLH[0]))
clon = np.cos(np.radians(satLLH[1]))
slon = np.sin(np.radians(satLLH[1]))
nhat = np.array([-clat * clon, -clat * slon, -slat])
temp = np.cross(nhat, velVec)
chat = temp / np.linalg.norm(temp)
temp = np.cross(chat, nhat)
that = temp / np.linalg.norm(temp)
vhat = velVec / np.linalg.norm(velVec)
zsch = height
# for ii in range(10):
###Near nadir tests
if (satLLH[2] - zsch) >= rng:
return None
a = (satLLH[2] + radius)
b = (radius + zsch)
costheta = 0.5 * (a / rng + rng / a - (b / a) * (b / rng))
sintheta = np.sqrt(1 - costheta * costheta)
gamma = rng * costheta
alpha = dopfact - gamma * np.dot(nhat, vhat) / np.dot(vhat, that)
beta = -side * np.sqrt(rng * rng * sintheta * sintheta - alpha * alpha)
delta = alpha * that + beta * chat + gamma * nhat
targVec = satVec + delta
targLLH = self.xyz_to_llh(list(targVec))
# targXYZ = self.llh_to_xyz([targLLH[0], targLLH[1], height])
# targSCH = self.xyz_to_sch(targXYZ)
#
# zsch = targSCH[2]
#
# rdiff = rng - np.linalg.norm(np.array(satVec) - np.array(targXYZ))
return targLLH
def geo2rdr(self, llh, azimuthTime_a, SatelliteOrbit_a, wvl, aux_referenceTime):
xyz = self.llh_to_xyz(llh)
tguess = azimuthTime_a[0]
if wvl == 0:
outOfBounds = False
for ii in range(51):
try:
sv = SatelliteOrbit_a.getSatelliteSpaceState(tguess) # 获取卫星的 位置、速度
except Exception as e:
print(e)
outOfBounds = True
break
pos = np.array(sv[0:3]) # 卫星坐标
vel = np.array(sv[3:6]) # 卫星速度
dr = xyz - pos
rng = np.linalg.norm(dr) # 求斜距
dopfact = np.dot(dr, vel) # fd 公式
fdop = 0 # doppler(DTU.seconds_since_midnight(tguess), rng) * wvl * 0.5
fdopder = 0 # (0.5 * wvl * doppler(DTU.seconds_since_midnight(tguess), rng + 10.0) - fdop) / 10.0
fn = dopfact - fdop * rng
c1 = -np.dot(vel, vel)
c2 = (fdop / rng + fdopder)
fnprime = c1 + c2 * dopfact
tguess = tguess - datetime.timedelta(seconds=fn / fnprime)
if outOfBounds:
raise Exception('Interpolation time out of bounds')
else:
dt = 0.0001
outOfBounds = False
for ii in range(51):
try:
sv = SatelliteOrbit_a.getSatelliteSpaceState(tguess + datetime.timedelta(seconds=dt)) # 获取卫星的 位置、速度
except Exception as e:
print(e)
outOfBounds = True
break
pos1 = np.array(sv[0:3]) # 卫星坐标
vel1 = np.array(sv[3:6]) # 卫星速度
dr1 = pos1 - xyz
rng1 = np.linalg.norm(dr1) # 斜距
FdTheory1 = -2 / (rng1 * wvl) * np.dot(dr1, vel1)
try:
sv = SatelliteOrbit_a.getSatelliteSpaceState(tguess)
except Exception as e:
print(e)
outOfBounds = True
break
pos2 = np.array(sv[0:3]) # 卫星坐标
vel2 = np.array(sv[3:6]) # 卫星速度
dr2 = pos2 - xyz
rng = np.linalg.norm(dr2) # 斜距
FdTheory2 = -2 / (rng * wvl) * np.dot(dr2, vel2)
TSR = rng * 2 / LIGHTSPEED - aux_referenceTime # nx1
FdNumerical = 0
fdopper_grad = (FdTheory1 - FdTheory2) / dt
inc_t = (FdTheory2 - FdNumerical) / fdopper_grad
tguess = tguess - datetime.timedelta(seconds=inc_t)
if abs(inc_t) < 1e-6:
break
else:
t_prev_guess = tguess
if outOfBounds:
raise Exception('Interpolation time out of bounds')
return tguess, rng
def getDoppler(self, taz, rng, azimuthTime, slantRange, xml_path):
with open(xml_path, 'r', encoding='utf-8') as fp:
HeaderFile_dom_str = fp.read()
HeaderFile_dom = xmltodict.parse(HeaderFile_dom_str) # 将XML转成json文本
HeaderFile_dom_json = json.loads(json.dumps(HeaderFile_dom))
DopplerRates = self.FindInfomationFromJson(HeaderFile_dom_json, ['level1Product','processing',"geometry", 'dopplerRate', 'dopplerRatePolynomial', 'coefficient'])
dr = float(self.FindInfomationFromJson(HeaderFile_dom_json, ['level1Product','productInfo','imageDataInfo', 'imageRaster','columnSpacing']).get('#text'))
DopplerRate = []
for doppler in DopplerRates:
DopplerRate.append(float(doppler.get('#text')))
dcoeffs = []
for ind, val in enumerate(DopplerRate):
dcoeffs.append(val / (dr ** ind))
coeffs = self.getCoeffs(dcoeffs)
dopplerFat = 0.
meanAzimuth = azimuthTime[0].timestamp()
normAzimuth = azimuthTime[-1].timestamp() - meanAzimuth
meanRange = slantRange[0]
normRange = slantRange[-1]
y = (taz - meanAzimuth) / normAzimuth
x = (rng - meanRange) / normRange
for ii, row in enumerate(coeffs):
yfact = y ** ii
for jj, col in enumerate(row):
dopplerFat += coeffs[ii][jj] * yfact * (x**jj)
return dopplerFat
def getCoeffs(self, dcoeffs):
coeffs = [[0. for i in j] for j in dcoeffs]
for ii, row in enumerate(dcoeffs):
for jj, col in enumerate(row):
coeffs[ii][jj] = float(col)
return coeffs
def getReferenceTime(self, xml_path):
root = ElementTree(file=xml_path).getroot()
referenceTime = float(root.find("processing").find("geometry").find("dopplerRate").find("dopplerRatePolynomial").find("referencePoint").text)
return referenceTime
def getENUHeading(self,SatelliteOrbit, time):
'''
Compute heading at given azimuth time using single state vector.
If time is not provided, mid point of orbit is used.
'''
vec1 = SatelliteOrbit.getSatelliteSpaceState(time)
llh1 = self.xyz_to_llh(vec1[0:3])
enumat = self.enubasis(llh1)
venu = np.dot(enumat.xyz_to_enu, vec1[3:6])
hdg = np.arctan2(venu[0, 0], venu[0, 1])
return np.degrees(hdg)
def enubasis(self, posLLH):
"""
xyzenuMat = elp.enubasis(posLLH)
Given an instance elp of an Ellipsoid LLH position (as a list) return the
transformation matrices from the XYZ frame to the ENU frame and the
inverse from the ENU frame to the XYZ frame.
The returned object is a namedtuple with numpy matrices in elements
named 'enu_to_xyz' and 'xyz_to_enu'
enu_to_xyzMat = (elp.enubasis(posLLH)).enu_to_xyz
xyz_to_enuMat = (elp.enubasis(posLLH)).xyz_to_enu
"""
import numpy
r_lat = numpy.radians(posLLH[0])
r_lon = numpy.radians(posLLH[1])
r_clt = numpy.cos(r_lat)
r_slt = numpy.sin(r_lat)
r_clo = numpy.cos(r_lon)
r_slo = numpy.sin(r_lon)
r_enu_to_xyzMat = numpy.matrix([
[-r_slo, -r_slt * r_clo, r_clt * r_clo],
[r_clo, -r_slt * r_slo, r_clt * r_slo],
[0.0, r_clt, r_slt]])
r_xyz_to_enuMat = r_enu_to_xyzMat.transpose()
enuxyzMat = namedtuple("enuxyzMat", "enu_to_xyz xyz_to_enu")
return enuxyzMat(r_enu_to_xyzMat, r_xyz_to_enuMat)
def setSCH(self, pegLat, pegLon, pegHdg, pegHgt=0.0):
"""
Set up an SCH coordinate system at the given peg point.
Set a peg point on the ellipse at pegLat, pegLon, pegHdg (in degrees).
Set the radius of curvature and the transformation matrix and offset
vector needed to transform between (s,c,h) and ecef (x,y,z).
"""
self.pegLat = pegLat
self.pegLon = pegLon
self.pegHdg = pegHdg
self.pegHgt = pegHgt
self.pegLLH = [pegLat, pegLon, pegHgt]
# determine the radius of curvature at the peg point, i.e, the
# the radius of the SCH sphere
self.pegRadCur = self.radiusOfCurvature(self.pegLLH, pegHdg)
# # determine the rotation matrix (from radar_to_xyz.F)
# r_lat = np.radians(pegLat)
# r_lon = np.radians(pegLon)
# r_hdg = np.radians(pegHdg)
# r_clt = np.cos(r_lat)
# r_slt = np.sin(r_lat)
# r_clo = np.cos(r_lon)
# r_slo = np.sin(r_lon)
# r_chg = np.cos(r_hdg)
# r_shg = np.sin(r_hdg)
#
# ptm11 = r_clt * r_clo
# ptm12 = -r_shg * r_slo - r_slt * r_clo * r_chg
# ptm13 = r_slo * r_chg - r_slt * r_clo * r_shg
# ptm21 = r_clt * r_slo
# ptm22 = r_clo * r_shg - r_slt * r_slo * r_chg
# ptm23 = -r_clo * r_chg - r_slt * r_slo * r_shg
# ptm31 = r_slt
# ptm32 = r_clt * r_chg
# ptm33 = r_clt * r_shg
#
# self.pegRotMatNP = np.matrix(
# [[ptm11, ptm12, ptm13],
# [ptm21, ptm22, ptm23],
# [ptm31, ptm32, ptm33]]
# )
# self.pegRotMatInvNP = self.pegRotMatNP.transpose()
#
# self.pegRotMat = self.pegRotMatNP.tolist()
# self.pegRotMatInv = self.pegRotMatInvNP.tolist()
#
# # find the translation vector as a column matrix
# self.pegXYZ = self.llh_to_xyz(self.pegLLH)
# self.pegXYZNP = np.matrix(self.pegXYZ).transpose()
#
# # Outward normal to ellispoid at the peg point
# self.pegNormal = [r_clt * r_clo, r_clt * r_slo, r_slt]
# self.pegNormalNP = np.matrix(self.pegNormal).transpose()
#
# # Offset Vector - to center of SCH sphere
# self.pegOVNP = self.pegXYZNP - self.pegRadCur * self.pegNormalNP
# self.pegOV = self.pegOVNP.transpose().tolist()[0]
return
def radiusOfCurvature(self, llh, hdg=0):
"""
radiusOfCurvature(llh,[hdg]): returns Radius of Curvature (meters)
in the direction specified by hdg for the instance ellipsoid
given a position llh=(lat (deg), lon (deg), h (meters)).
If no heading is given the default is 0, or North.
"""
r_lat = math.radians(llh[0])
r_hdg = math.radians(hdg)
reast = self.eastRadiusOfCurvature(llh)
rnorth = self.northRadiusOfCurvature(llh)
# radius of curvature for point on ellipsoid
rdir = (reast * rnorth) / (
reast * math.cos(r_hdg) ** 2 + rnorth * math.sin(r_hdg) ** 2)
# add height of the llh point
return rdir + llh[2]
def eastRadiusOfCurvature(self, llh):
"""eastRadiusOfCurvature(llh): returns Radius of Curvature (meters) \nin the East direction for the instance
ellipsoid \ngiven a position llh=(lat (deg), lon (deg), h (meters))"""
r_lat = math.radians(llh[0])
reast = reference_ellipsoid_params['a'] / math.sqrt(1.0 - e2 * math.sin(r_lat) ** 2)
return reast
##
# Compute the radius of curvature in the north direction on an ellipsoid
def northRadiusOfCurvature(self, llh):
"""northRadiusOfCurvature(llh): returns Radius of Curvature (meters) \nin the North direction for the instance
ellipsoid \ngiven a position llh=(lat (deg), lon (deg), h (meters))"""
r_lat = math.radians(llh[0])
rnorth = (reference_ellipsoid_params['a'] * (1.0 - e2)) / (1.0 - e2 * math.sin(r_lat) ** 2) ** (1.5)
return rnorth
# 距离方程计算
@staticmethod
def compute_distance(slant_range, doppler_frequency):
c = 3 * 10**8 # 光速
distance = (c * doppler_frequency) / (2 * slant_range)
return distance
# 高度方程（假设直接投影到地球表面）
@staticmethod
def compute_height(slant_range, incident_angle, reference_ellipsoid_params):
a = reference_ellipsoid_params['a']
b = reference_ellipsoid_params['b']
q = np.sqrt((slant_range**2))
w = ((a**2) * (np.sin(incident_angle)**2))
height = np.sqrt((slant_range**2) - ((a**2) * (np.sin(incident_angle)**2))) - b
return height
# 计算P点在参考椭球面上的等斜距投影点P0的坐标
@staticmethod
def compute_P0_coordinates(latitude_P, longitude_P, height_P0):
a = reference_ellipsoid_params['a']
e_squared = 1 - ((reference_ellipsoid_params['b']**2) / (reference_ellipsoid_params['a']**2))
N = a / math.sqrt(1 - e_squared * math.sin(math.radians(latitude_P))**2)
X = (N + height_P0) * math.cos(math.radians(latitude_P)) * math.cos(math.radians(longitude_P))
Y = (N + height_P0) * math.cos(math.radians(latitude_P)) * math.sin(math.radians(longitude_P))
Z = (N * (1 - e_squared) + height_P0) * math.sin(math.radians(latitude_P))
return X, Y, Z
if __name__ == '__main__':
sGps = SARgps()
# 假设P点的经纬度已知
latitude_P = 40.0 # P点纬度
longitude_P = -100.0 # P点经度
h = 5000
xyz = sGps.llh_to_xyz([latitude_P, longitude_P, h])
print(xyz)
llh = sGps.xyz_to_llh(xyz)
print(llh)
distance_P = sGps.compute_distance(slant_range, doppler_frequency)
# 方位方程（假设一些参数）
incident_angle = 30 # 入射角
azimuth = math.radians(45) # 方位角
height_P0 = sGps.compute_height(slant_range, incident_angle, reference_ellipsoid_params)
P0_coordinates = sGps.compute_P0_coordinates(latitude_P, longitude_P, height_P0)
print("P0点在参考椭球面上的等斜距投影点坐标:", P0_coordinates)