800 lines
33 KiB
Python
800 lines
33 KiB
Python
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)
|
||
|
||
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()
|