---

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