From 76ceea85a4b570a318ba338322f7d247b761761b Mon Sep 17 00:00:00 2001 From: shuibing811 Date: Fri, 7 Feb 2025 10:00:09 +0800 Subject: [PATCH] --- --- main.py | 345 +++++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 252 insertions(+), 93 deletions(-) diff --git a/main.py b/main.py index bb0edc5..bc81be7 100644 --- a/main.py +++ b/main.py @@ -33,7 +33,7 @@ def _(lla2ecef, np): self.pt = pt self.gt = gt self.pw = pw - class RadarPdw: + class RadarPdw: def __init__(self): pass return Radar, RadarPdw, Station, Target @@ -218,10 +218,10 @@ def _(RadarPdw, lla2enu, math, np, radars, stations, targets): radar_pdws[i].pa = np.sqrt(radar_pdws[i].pr) radar_pdws[i].label = np.ones(pulse_num)*i #此处从0开始，matlab从1开始 return radar_pdws - + def calc_propagation_time(radar_pos,station_pos): return np.linalg.norm(radar_pos-station_pos)/3e8 - + def calc_doa_north(pos1,pos2): x1 = pos1[0] y1 = pos1[1] @@ -323,8 +323,6 @@ def _(): #generate_rf({"type": "STB", "center": 1.1e9, "bw": 0.0},100) #RF_Generate("Agility",5.1e9,100,0.6e9) #generate_rf({"type": "Agility", "center": 5.1e9, "bw": 0.6e9},100) - - return @@ -337,11 +335,11 @@ def _(locate, radar_pdws): return (radar_pdws_located,) -app._unparsable_cell( - r""" +@app.cell +def _(distance, lla2enu, ned2lla, np, stations, targets): def locate(radar_pdws): - # values = sci.io.loadmat(\"radar_pdws.mat\") - # radar_pdws = values[\"radar_pdws\"] + # values = sci.io.loadmat("radar_pdws.mat") + # radar_pdws = values["radar_pdws"] # print(len(radar_pdws)) stations_enu = np.empty((len(stations),3)) for i in range(len(stations)): @@ -352,8 +350,8 @@ app._unparsable_cell( location_estimated = np.empty((len(doa),3)) for j in range(len(doa)): ned = least_square(doa[j],stations_enu[:,0],stations_enu[:,1]) - lat,lon,_ = ned2lla(ned[0],ned[1],0,*stations[0].lla) - erro[j] = distance(targets[0].lla[0],targets[0].lla[1],lat,lon) + lat,lon,_ = ned2lla(ned[0],ned[1],0,*stations[0].lla) + erro[j],_ = distance(targets[0].lla[0],targets[0].lla[1],lat,lon) location_estimated[j,:] = np.array([lat,lon,0]) radar_pdws[i].location = np.empty((len(radar_pdws[i].pri),3)) @@ -372,19 +370,18 @@ app._unparsable_cell( hmtx = np.hstack((-np.tan(doa_rad).T,np.ones((l,1)))) ned = np.linalg.inv(hmtx.T @ hmtx) @ hmtx.T @ fmtx.T #确定是ned？ return ned.flat - """, - name="_" -) + return least_square, locate @app.cell -def _(dist, np): +def _(np): import pyproj import scipy.spatial.transform from scipy.spatial.transform import Rotation as R from geopy.distance import geodesic + from math import radians, degrees, sin, cos, atan2, sqrt, acos - def lla2ecef(lat,lon,alt): + def lla2ecef(lat,lon,alt): """ 将经纬度高度（LLA）转换为地心地固坐标系（ECEF）坐标。 @@ -405,98 +402,249 @@ def _(dist, np): # 进行坐标转换 x, y, z = transformer.transform(lat, lon, alt, radians=False) return x, y, z - #TODO：提升效率 - def lla2enu(lat, lon, alt, lat_org, lon_org, alt_org): + + def ecef2lla(x,y,z): + # 定义源坐标系统（WGS84 经纬度） + lla = pyproj.CRS("EPSG:4326") + # 定义目标坐标系统（ECEF） + ecef = pyproj.CRS("EPSG:4978") + # 创建转换器对象 + transformer = pyproj.Transformer.from_crs(ecef,lla) + # 进行坐标转换 + lat, lon, alt = transformer.transform(x, y, z) + return lat, lon, alt + + def lla2enu(lat, lon, alt, ref_lat, ref_lon, ref_alt): """ - 将 LLA 坐标转换为 ENU 坐标 - :param lat: 目标点的纬度（单位：度） - :param lon: 目标点的经度（单位：度） - :param alt: 目标点的高度（单位：米） - :param lat_org: 参考点的纬度（单位：度） - :param lon_org: 参考点的经度（单位：度） - :param alt_org: 参考点的高度（单位：米） - :return: 转换后的 ENU 坐标（东、北、天） + 将 LLA 转换为 ENU 坐标 + :param lat: 点的纬度 (°) + :param lon: 点的经度 (°) + :param alt: 点的海拔 (m) + :param ref_lat: 参考点纬度 (°) + :param ref_lon: 参考点经度 (°) + :param ref_alt: 参考点海拔 (m) + :return: ENU 坐标 (East, North, Up) """ - # 定义转换器 - transformer1 = pyproj.Transformer.from_crs( - {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, - {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, - ) - transformer2 = pyproj.Transformer.from_crs( - {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, - {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, - ) - - # 将目标点和参考点的 LLA 坐标转换为 ECEF 坐标 - x, y, z = transformer1.transform(lon, lat, alt, radians=False) - x_org, y_org, z_org = transformer1.transform(lon_org, lat_org, alt_org, radians=False) - - # 计算 ECEF 坐标差 - ecef_delta = np.array([x - x_org, y - y_org, z - z_org]) - - # 构造旋转矩阵 - rot1 = scipy.spatial.transform.Rotation.from_euler('x', -(90 - lat_org), degrees=True).as_matrix() - rot3 = scipy.spatial.transform.Rotation.from_euler('z', -(90 + lon_org), degrees=True).as_matrix() - rot_matrix = rot1.dot(rot3) - - # 将 ECEF 坐标差转换为 ENU 坐标 - enu = rot_matrix.dot(ecef_delta) - + # 将参考点和目标点的 LLA 转为 ECEF 坐标 + ref_ecef = np.array(lla2ecef(ref_lat, ref_lon, ref_alt)) + point_ecef = np.array(lla2ecef(lat, lon, alt)) + + # 计算 ECEF 差值 + delta = point_ecef - ref_ecef + + # 构建 ENU 转换矩阵 + ref_lat, ref_lon = np.radians(ref_lat), np.radians(ref_lon) + t = np.array([ + [-np.sin(ref_lon), np.cos(ref_lon), 0], + [-np.sin(ref_lat) * np.cos(ref_lon), -np.sin(ref_lat) * np.sin(ref_lon), np.cos(ref_lat)], + [np.cos(ref_lat) * np.cos(ref_lon), np.cos(ref_lat) * np.sin(ref_lon), np.sin(ref_lat)] + ]) + + # 计算 ENU 坐标 + enu = t @ delta return enu - #TODO：提升效率 - def enu2lla(x, y, z, lat_org, lon_org, alt_org): + + def enu2lla(e, n, u, ref_lat, ref_lon, ref_alt): """ - 将 ENU 坐标转换为 LLA 坐标 - :param x: ENU 坐标中的东向分量 - :param y: ENU 坐标中的北向分量 - :param z: ENU 坐标中的天向分量 - :param lat_org: 参考点的纬度（单位：度） - :param lon_org: 参考点的经度（单位：度） - :param alt_org: 参考点的高度（单位：米） - :return: 转换后的 LLA 坐标（纬度、经度、高度） + 将 ENU 转换为 LLA 坐标 + :param e: ENU 坐标的 East 分量 (m) + :param n: ENU 坐标的 North 分量 (m) + :param u: ENU 坐标的 Up 分量 (m) + :param ref_lat: 参考点纬度 (°) + :param ref_lon: 参考点经度 (°) + :param ref_alt: 参考点海拔 (m) + :return: LLA 坐标 (Latitude, Longitude, Altitude) """ - # 定义转换器 - transformer1 = pyproj.Transformer.from_crs( - {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, - {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, - ) - transformer2 = pyproj.Transformer.from_crs( - {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, - {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, - ) + # 将参考点的 LLA 转为 ECEF 坐标 + ref_ecef = lla2ecef(ref_lat, ref_lon, ref_alt) - # 将参考点转换为 ECEF 坐标 - x_org, y_org, z_org = transformer1.transform(lon_org, lat_org, alt_org, radians=False) - ecef_org = np.array([[x_org, y_org, z_org]]).T + # 构建 ENU 转换矩阵 + ref_lat, ref_lon = np.radians(ref_lat), np.radians(ref_lon) + t = np.array([ + [-np.sin(ref_lon), np.cos(ref_lon), 0], + [-np.sin(ref_lat) * np.cos(ref_lon), -np.sin(ref_lat) * np.sin(ref_lon), np.cos(ref_lat)], + [np.cos(ref_lat) * np.cos(ref_lon), np.cos(ref_lat) * np.sin(ref_lon), np.sin(ref_lat)] + ]) - # 构造旋转矩阵 - rot1 = scipy.spatial.transform.Rotation.from_euler('x', -(90 - lat_org), degrees=True).as_matrix() - rot3 = scipy.spatial.transform.Rotation.from_euler('z', -(90 + lon_org), degrees=True).as_matrix() - rot_matrix = rot1.dot(rot3) + # 计算 ECEF 坐标 + delta = np.linalg.inv(t) @ np.array([e, n, u]) + point_ecef = ref_ecef + delta - # 将 ENU 坐标转换为 ECEF 坐标 - ecef_delta = rot_matrix.T.dot(np.array([[x, y, z]]).T) - ecef = ecef_delta + ecef_org + # 将 ECEF 坐标转换为 LLA + lla = ecef2lla(*point_ecef) + return lla + + #TODO：比较慢，需提升效率 + # def lla2enu(lat, lon, alt, lat_org, lon_org, alt_org): + # """ + # 将 LLA 坐标转换为 ENU 坐标 + # :param lat: 目标点的纬度（单位：度） + # :param lon: 目标点的经度（单位：度） + # :param alt: 目标点的高度（单位：米） + # :param lat_org: 参考点的纬度（单位：度） + # :param lon_org: 参考点的经度（单位：度） + # :param alt_org: 参考点的高度（单位：米） + # :return: 转换后的 ENU 坐标（东、北、天） + # """ + # # 定义转换器 + # transformer1 = pyproj.Transformer.from_crs( + # {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, + # {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, + # ) + # transformer2 = pyproj.Transformer.from_crs( + # {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, + # {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, + # ) - # 将 ECEF 坐标转换为 LLA 坐标 - lon, lat, alt = transformer2.transform(ecef[0, 0], ecef[1, 0], ecef[2, 0], radians=False) + # # 将目标点和参考点的 LLA 坐标转换为 ECEF 坐标 + # x, y, z = transformer1.transform(lon, lat, alt, radians=False) + # x_org, y_org, z_org = transformer1.transform(lon_org, lat_org, alt_org, radians=False) + + # # 计算 ECEF 坐标差 + # ecef_delta = np.array([x - x_org, y - y_org, z - z_org]) + + # # 构造旋转矩阵 + # rot1 = scipy.spatial.transform.Rotation.from_euler('x', -(90 - lat_org), degrees=True).as_matrix() + # rot3 = scipy.spatial.transform.Rotation.from_euler('z', -(90 + lon_org), degrees=True).as_matrix() + # rot_matrix = rot1.dot(rot3) + + # # 将 ECEF 坐标差转换为 ENU 坐标 + # enu = rot_matrix.dot(ecef_delta) + + # return enu + + #TODO：比较慢，需提升效率 + # def enu2lla(x, y, z, lat_org, lon_org, alt_org): + # """ + # 将 ENU 坐标转换为 LLA 坐标 + # :param x: ENU 坐标中的东向分量 + # :param y: ENU 坐标中的北向分量 + # :param z: ENU 坐标中的天向分量 + # :param lat_org: 参考点的纬度（单位：度） + # :param lon_org: 参考点的经度（单位：度） + # :param alt_org: 参考点的高度（单位：米） + # :return: 转换后的 LLA 坐标（纬度、经度、高度） + # """ + # # 定义转换器 + # transformer1 = pyproj.Transformer.from_crs( + # {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, + # {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, + # ) + # transformer2 = pyproj.Transformer.from_crs( + # {"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'}, + # {"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'}, + # ) + + # # 将参考点转换为 ECEF 坐标 + # x_org, y_org, z_org = transformer1.transform(lon_org, lat_org, alt_org, radians=False) + # ecef_org = np.array([[x_org, y_org, z_org]]).T + + # # 构造旋转矩阵 + # rot1 = scipy.spatial.transform.Rotation.from_euler('x', -(90 - lat_org), degrees=True).as_matrix() + # rot3 = scipy.spatial.transform.Rotation.from_euler('z', -(90 + lon_org), degrees=True).as_matrix() + # rot_matrix = rot1.dot(rot3) + + # # 将 ENU 坐标转换为 ECEF 坐标 + # ecef_delta = rot_matrix.T.dot(np.array([[x, y, z]]).T) + # ecef = ecef_delta + ecef_org + + # # 将 ECEF 坐标转换为 LLA 坐标 + # lon, lat, alt = transformer2.transform(ecef[0, 0], ecef[1, 0], ecef[2, 0], radians=False) + + # return [lat, lon, alt] - return [lat, lon, alt] def lla2ned(lat, lon, alt,lat_ref, lon_ref, alt_ref): [east, north, up] = lla2enu(lat, lon, alt,lat_ref, lon_ref, alt_ref) return north, east, -up - + def ned2lla(n, e, d, lat0, lon0, alt0): lla = enu2lla(e, n, -d,lat0,lon0,alt0) return lla[0],lla[1],lla[2] - #TODO：要和matlab中的distance实现一致 + # def ned2lla(n, e, d, lat0, lon0, alt0): + # """将NED位移转换为LLA坐标""" + # # 参考点转ECEF + # x0, y0, z0 = lla2ecef(lat0, lon0, alt0) + + # # 参考点经纬度（弧度） + # lat0_rad = math.radians(lat0) + # lon0_rad = math.radians(lon0) + + # # 构建旋转矩阵 + # sin_lat = math.sin(lat0_rad) + # cos_lat = math.cos(lat0_rad) + # sin_lon = math.sin(lon0_rad) + # cos_lon = math.cos(lon0_rad) + + # # 北、东、地方向的单位向量 + # N_x = -sin_lat * cos_lon + # N_y = -sin_lat * sin_lon + # N_z = cos_lat + + # E_x = -sin_lon + # E_y = cos_lon + # E_z = 0 + + # D_x = -cos_lat * cos_lon + # D_y = -cos_lat * sin_lon + # D_z = -sin_lat + + # # 计算ECEF中的位移 + # dx = N_x * n + E_x * e + D_x * d + # dy = N_y * n + E_y * e + D_y * d + # dz = N_z * n + E_z * e + D_z * d + + # # 目标点ECEF坐标 + # x = x0 + dx + # y = y0 + dy + # z = z0 + dz + + # # 转换回LLA + # lat, lon, alt = ecef2lla(x, y, z) + + # return (lat, lon, alt) + def distance(lat1, lon1, lat2, lon2): - return geodesic((lat1, lon1), (lat2, lon2)).miles #.kilometers + # 将经纬度从度转换为弧度 + lat1, lon1, lat2, lon2 = map(radians, [lat1, lon1, lat2, lon2]) + + # 计算球面角距离（arclen，单位为度数） + delta_lon = lon2 - lon1 + delta_sigma = acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(delta_lon)) + arclen = degrees(delta_sigma) # 将弧度转换为角度 + + # 计算初始方位角（az，单位为度数） + x = sin(delta_lon) * cos(lat2) + y = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_lon) + az = degrees(atan2(x, y)) # 转换为角度 + az = (az + 360) % 360 # 将方位角调整为 [0, 360) + + return arclen, az + + #TODO：要和matlab中的distance实现一致 + # def distance(lat1, lon1, lat2, lon2): + # distance = geodesic((lat1, lon1), (lat2, lon2)).kilometers #.miles .kilometers + + # # 将经纬度从度转换为弧度 + # lat1_rad = math.radians(lat1) + # lon1_rad = math.radians(lon1) + # lat2_rad = math.radians(lat2) + # lon2_rad = math.radians(lon2) + + # # 计算方位角 + # d_lon = lon2_rad - lon1_rad + # y = math.sin(d_lon) * math.cos(lat2_rad) + # x = math.cos(lat1_rad) * math.sin(lat2_rad) - math.sin(lat1_rad) * math.cos(lat2_rad) * math.cos(d_lon) + # azimuth = math.degrees(math.atan2(y, x)) + # # 将方位角转换为 0 到 360 度的范围 + # azimuth = (azimuth + 360) % 360 + + # return distance/1.852, azimuth + # geod = pyproj.Geod(ellps="WGS84") # 使用 WGS84 椭球体 # _, _, dist = geod.inv(lon1, lat1, lon2, lat2) # 计算距离 - return dist + # return dist # def haversine_distance(lat1, lon1, lat2, lon2): # """ @@ -527,6 +675,7 @@ def _(dist, np): # # 计算距离 # distance = R * c + # return distance # return distance # def haversine_distance(lat1, lon1, lat2, lon2): @@ -567,10 +716,15 @@ def _(dist, np): # # Calculate the result # distance = c * r - # return + # return distance return ( R, + acos, + atan2, + cos, + degrees, distance, + ecef2lla, enu2lla, geodesic, lla2ecef, @@ -578,16 +732,23 @@ def _(dist, np): lla2ned, ned2lla, pyproj, + radians, scipy, + sin, + sqrt, ) @app.cell -def _(distance, enu2lla, lla2ecef, lla2enu, lla2ned, ned2lla): +def _(distance, ecef2lla, enu2lla, lla2ecef, lla2enu, lla2ned, ned2lla): #lla2ecef([30.0,120.0,100.0]) X, Y, Z = lla2ecef(30.0, 120.0, 100.0) #ok print(f"lla2ecef： (X, Y, Z): ({X:.2f}, {Y:.2f}, {Z:.2f}) 米") + #ecef2lla([-2764171.62, 4787685.69, 3170423.74]) + lat,lon,alt = ecef2lla(-2764171.62, 4787685.69, 3170423.74) + print(f"ecef2lla： (lat, lon, alt): ({lat:.2f}, {lon:.2f}, {alt:.2f})") + # lla2enu([30.0,120.0, 120.0], [29.0,119.0, 50.0],'ellipsoid') e, n, u = lla2enu(30.0,120.0, 120.0, 29.0,119.0, 50.0) #ok print(f"lla2enu: E = {e} m, N = {n} m, U = {u} m") @@ -608,15 +769,13 @@ def _(distance, enu2lla, lla2ecef, lla2enu, lla2ned, ned2lla): print(f"ned2lla：lat={lat:.6f}°, lon={lon:.6f}°, alt={alt:.2f}") #distance(34.0522, -118.2437, 40.7128, -74.0060) - print(distance(34.0522, -118.2437, 40.7128, -74.0060)) + print("distance :" ,distance(34.0522, -118.2437, 40.7128, -74.0060)) + ##print("haversine_distance :", haversine_distance(34.0522, -118.2437, 40.7128, -74.0060)) return X, Y, Z, alt, d, e, lat, lla, lon, n, u @app.cell def _(): - - - return