temp/RadarSingalAnalyzer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

---

Browse Source
This commit is contained in:
shuibing811
2025-01-25 16:26:42 +08:00
parent d0932b7d18
commit f3cfdb677b
2 changed files with 332 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
crosslocation.m
View File

@@ -2,37 +2,37 @@ close all
clear all
station = [31.2304, 121.4737,0;
31.5200, 121.6400,0
];
Target = [31.2550, 121.4890,0; % Ŀ<EFBFBD><EFBFBD>1 ½
31.2800, 121.5200,0; % Ŀ<EFBFBD><EFBFBD>2 ½
31.1900, 121.4800,0; % Ŀ<EFBFBD><EFBFBD>3 ½
31.2700, 121.5500,0; % Ŀ<EFBFBD><EFBFBD>4 ½
31.2000, 121.6000,0; % Ŀ<EFBFBD><EFBFBD>5 ½
30.9500, 121.6500,0; % Ŀ<EFBFBD><EFBFBD>6 <EFBFBD><EFBFBD>
30.8500, 121.7200,0; % Ŀ<EFBFBD><EFBFBD>7 <EFBFBD><EFBFBD>
30.7800, 121.8000,0; % Ŀ<EFBFBD><EFBFBD>8 <EFBFBD><EFBFBD>
30.9200, 121.8500,0; % Ŀ<EFBFBD><EFBFBD>9 <EFBFBD><EFBFBD>
30.6700, 121.9000,0; % Ŀ<EFBFBD><EFBFBD>10 <EFBFBD><EFBFBD>
];
num = size(station,1); % <EFBFBD>۲<EFBFBD>վ<EFBFBD><EFBFBD>Ŀ
31.5200, 121.6400,0
];
Target = [31.2550, 121.4890,0; % Ŀ<EFBFBD><EFBFBD>1 ½
31.2800, 121.5200,0; % Ŀ<EFBFBD><EFBFBD>2 ½
31.1900, 121.4800,0; % Ŀ<EFBFBD><EFBFBD>3 ½
31.2700, 121.5500,0; % Ŀ<EFBFBD><EFBFBD>4 ½
31.2000, 121.6000,0; % Ŀ<EFBFBD><EFBFBD>5 ½
30.9500, 121.6500,0; % Ŀ<EFBFBD><EFBFBD>6 <EFBFBD><EFBFBD>
30.8500, 121.7200,0; % Ŀ<EFBFBD><EFBFBD>7 <EFBFBD><EFBFBD>
30.7800, 121.8000,0; % Ŀ<EFBFBD><EFBFBD>8 <EFBFBD><EFBFBD>
30.9200, 121.8500,0; % Ŀ<EFBFBD><EFBFBD>9 <EFBFBD><EFBFBD>
30.6700, 121.9000,0; % Ŀ<EFBFBD><EFBFBD>10 <EFBFBD><EFBFBD>
];
num = size(station,1); % <EFBFBD>۲<EFBFBD>վ<EFBFBD><EFBFBD>Ŀ
load('RadarPDW_pri_modulation.mat');%<EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><EFBFBD><EFBFBD>
for i = 1:num
[station1_enu(i,1) ,station1_enu(i,2) , station1_enu(i,3)]= lla2enu(station(i,:),station(1,:)); % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
[station1_enu(i,1) ,station1_enu(i,2) , station1_enu(i,3)]= lla2enu(station(i,:),station(1,:)); % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
end
for idx = 1:length(RadarPDW)
DOA = RadarPDW(idx).DOA;
DOA = RadarPDW(idx).DOA;
for i = 1 : length(DOA)
[Xls1_ned] = LeastSquare(DOA(i,:) ,station1_enu(:,1)',station1_enu(:,2)');
[radi(1,1),radi(1,2)] = ned2lla([Xls1_ned(1),Xls1_ned(2),0],station(1,:)); % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
erro(i) = distance(Target(1,1),Target(1,2),radi(1,1),radi(1,2)); % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
location_estimated(i,:) = [radi(1,1),radi(1,2),0];
[Xls1_ned] = LeastSquare(DOA(i,:) ,station1_enu(:,1)',station1_enu(:,2)');
[radi(1,1),radi(1,2)] = ned2lla([Xls1_ned(1),Xls1_ned(2),0],station(1,:)); % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
erro(i) = distance(Target(1,1),Target(1,2),radi(1,1),radi(1,2)); % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
location_estimated(i,:) = [radi(1,1),radi(1,2),0];
end
for i = 1:length(RadarPDW(idx).PRI)
RadarPDW(idx).location(i ,:) = location_estimated(i,:);
RadarPDW(idx).erro(i ,:) = erro(i);
RadarPDW(idx).location(i ,:) = location_estimated(i,:);
RadarPDW(idx).erro(i ,:) = erro(i);
end
end
@@ -52,7 +52,7 @@ save(filename, 'RadarPDW');
% load coastlines;
% plotm(coastlat,coastlon);
% hold on
% % scatterm(x_end_lla(1,:),x_end_lla(2,:),15,'filled','ro');
% % scatterm(x_end_lla(1,:),x_end_lla(2,:),15,'filled','ro');
% % scatterm(10,115,10,'filled','bo');
% geoshow(x_end_lla(1,:),x_end_lla(2,:),'Marker','.','MarkerEdgeColor','red','MarkerSize',15);%<EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
% geoshow(latitude,longitude,'Marker','.','MarkerEdgeColor','blue','MarkerSize',10);%<EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Դʵ<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>

433
main.py
View File

@@ -10,7 +10,8 @@ def _():
import math
import matplotlib.pyplot as plt
import marimo as mo
return math, mo, np, plt
import scipy as sci
return math, mo, np, plt, sci
@app.cell
@@ -46,7 +47,7 @@ def _(Radar, Station, Target):
Target(31.2800, 121.5200, 0), # 目标2 陆
Target(31.1900, 121.4800, 0), # 目标3 陆
Target(31.2700, 121.5500, 0), # 目标4 陆
# Target(31.2000, 121.6000, 0), # 目标5 陆
Target(31.2000, 121.6000, 0), # 目标5 陆
Target(30.9500, 121.6500, 0), # 目标6 海
Target(30.8500, 121.7200, 0), # 目标7 海
Target(30.7800, 121.8000, 0), # 目标8 海
@@ -104,6 +105,16 @@ def _(Radar, Station, Target):
# gt=1e3,
# pw=3e-6,
# ),
Radar(
31.2000,
121.6000,
0,
pri_info={"type": "STA", "values": [1e-3]},
rf_info={"type": "Agility", "center": 4e9, "bw": 0.6e9},
pt=5e3,
gt=1e3,
pw=3e-6,
),
Radar(
30.9500,
121.6500,
@@ -160,19 +171,22 @@ def _(Radar, Station, Target):
@app.cell
def _(
RadarPdw,
calc_doa_north,
calc_propagation_time,
generate_pri,
generate_rf,
lla2enu,
math,
np,
radars,
stations,
targets,
):
def _(generate_pdw, sci):
radar_pdws = generate_pdw()
##print(radar_pdws[0].pri)
#print(radar_pdws[0].toa)
#print(radar_pdws[0].doa)
#print(radar_pdws[0].pt)
#print(radar_pdws[0].pr)
#print(radar_pdws[0].pa)
#print(radar_pdws[0].label)
sci.io.savemat("radar_pdws.mat",{"radar_pdws":radar_pdws})
#保存radar_pdws
return (radar_pdws,)
@app.cell
def _(RadarPdw, lla2enu, math, np, radars, stations, targets):
def generate_pdw():
time = 2 # 仿真时间2s
c = 3e8
@@ -186,15 +200,15 @@ def _(
pulse_num = len(toa)
radar_pdws[i].pulse_num = pulse_num
radar_pdws[i].pri = radar_pdws[i].pri + radar_pdws[i].pri * np.random.normal(size=pulse_num) * 0.01
radar_pdws[i].doa = [[],[]] #两个站点doa
radar_pdws[i].doa = np.array([np.empty(pulse_num),np.empty(pulse_num)]) #两个站点doa
#station1 doa
station1_enu = lla2enu(*stations[0].lla.tolist(),*targets[i].lla.tolist())
doa = calc_doa_north(station1_enu,[0,0,0])
radar_pdws[i].doa[0] = doa + np.random.normal(size=pulse_num)/1000
radar_pdws[i].doa[0,:] = doa + np.random.normal(size=pulse_num)/1000
#station2 doa
station2_enu = lla2enu(*stations[1].lla.tolist(),*targets[i].lla.tolist())
doa = calc_doa_north(station2_enu,[0,0,0])
radar_pdws[i].doa[1] = doa + np.random.normal(size=pulse_num)/1000
radar_pdws[i].doa[1,:] = doa + np.random.normal(size=pulse_num)/1000
radar_pdws[i].rf = generate_rf(radar.rf_info,pulse_num)
radar_pdws[i].pt = radar.pt + radar.pt * np.random.normal(size=pulse_num) * 0.001
@@ -202,64 +216,12 @@ def _(
lambda_ = c / radar_pdws[i].rf
radar_pdws[i].pr = radar.pt * lambda_**2 * radar.gt * gr / (4*math.pi*r)**2
radar_pdws[i].pa = np.sqrt(radar_pdws[i].pr)
radar_pdws[i].label = np.ones(pulse_num)*i
radar_pdws[i].label = np.ones(pulse_num)*i #此处从0开始，matlab从1开始
return radar_pdws
return (generate_pdw,)
app._unparsable_cell(
r"""
def locate():
stations_enu = np.empty((len(stations),3))
for i in len(stations):
stations_enu[i,:] = np.array(lla2enu(stations[i].lla,stations[0]))
for i in len(radar_pdws):
doa = radar_pdws[i].doa
erro = np.empty(len(doa))
location_estimated = np.empty(len(doa))
for j in len(doa):
ned = least_square(doa[j],stations_enu(:,0),stations_enu(:,1))
radi = ned2lla([ned[0],ned[1],0],stations[0].lla)
erro[j] = distance(targets[0].lla[0],targets[0].lla[1],radi[0],radi[1])
location_estimated[j] = np.array([radi(0),radi(1),0])
for j in len(radar_pdws[i].pri):
radar_pdws[i].location[j,:] = location_estimated[j,:]
radar_pdws[i].erro[j,:] = erro[j]
return radar_pdws
""",
name="_"
)
@app.cell
def _(generate_pdw, locate, 保存radar_pdws):
radar_pdws = generate_pdw()
#保存radar_pdws
radar_pdws_located = locate()
保存radar_pdws
return radar_pdws, radar_pdws_located
@app.cell
def _(math, np):
def distance(lat1,lon1,lat2,lon2):
pass
def least_square(doa,e,n):
doa_rad = np.deg2rad(doa)
l = len(doa_rad)
ee = e[0:l]
nn = n[0:l]
fmtx = ee - nn * np.tan(doa_rad)
hmtx = np.hstack(-np.tan(doa_rad).T,np.ones(l).T)
ned = (hmtx.T @ hmtx) ** -1 * hmtx.T * fmtx.T
return ned
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]
@@ -296,22 +258,21 @@ def _(math, np):
pri_value = pri_info["values"][0]
n = round(time/pri_value)
m = 10
pri = np.full(m,0)
pri = np.empty(m)
for i in range(m):
gamma = (0.01 + (0.2 - 0.01) * np.random.rand()) * [-1.0,1.0][np.random.randint(2)]
pri[i] = pri_value + gamma * pri_value
pri = np.tile(pri, (1, math.ceil(n/m)))
pri = np.tile(pri, math.ceil(n/m))
pri = pri[0:n]
toa = np.cumsum(pri)
return toa,pri
elif pri_info["type"] == "STA": #参差(Staggered)
pri_values = pri_info["values"]
print(pri_values)
pri_avg = np.mean(pri_values)
n = round(time/pri_avg)
m = len(pri_values)
pri = np.tile(pri_values,(1,math.ceil(n/m)))
pri = np.tile(pri_values,math.ceil(n/m))
pri = pri[0:n]
toa = np.cumsum(pri)
return toa,pri
@@ -319,16 +280,13 @@ def _(math, np):
elif pri_info["type"] == "SLI": #滑变Sliding
pri_values = pri_info["values"]
pri_avg = np.mean(pri_values)
n = round(time/pri_avg)
m = len(pri_values)
pri_min = pri_values[0]
pri_max = pri_values[1]
n = round(time/pri_avg)
m = 25
delta = (pri_max-pri_min)/m
pri = np.full(m,0)
for i in range(m):
pri[i] = pri_min + delta*i
pri = np.tile(pri,(1,math.ceil(n/m)))
pri = np.array([pri_min + delta * i for i in range(m)])
pri = np.tile(pri,math.ceil(n/m))
pri = pri[0:n]
toa = np.cumsum(pri)
return toa,pri
@@ -337,16 +295,93 @@ def _(math, np):
return (
calc_doa_north,
calc_propagation_time,
distance,
generate_pdw,
generate_pri,
generate_rf,
least_square,
)
@app.cell
def _():
#test
#propagation_time([12,33,44],[123,44,55])
#calc_propagation_time(np.array([12,33,44]),np.array([123,44,55])) #ok
#CalculatDoaNorth([12,33,44],[123,44,55])
#calc_doa_north([12,33,44],[123,44,55]) #ok
#PRI_Generate3("JTR",2,2918e-6)
#print(generate_pri({"type": "JTR", "values": [2918e-6]},2)) #ok
#PRI_Generate3("STB",2,1377e-6)
#print(generate_pri({"type": "STB", "values": [1377e-6]},2)) #ok
#PRI_Generate3("STA",2,1e-3)
##print(generate_pri({"type": "STA", "values": [1e-3]},2)) #ok
#PRI_Generate3("SLI",2,[1800e-6, 3800e-6])
#print(generate_pri({"type": "SLI", "values": [1800e-6, 3800e-6]},2)) #ok
#RF_Generate("STB",1.1e9,100,0)
#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
@app.cell
def _(locate, radar_pdws):
radar_pdws_located = locate(radar_pdws)
#保存radar_pdws
#sci.io.savemat("radar_pdws_locate.mat",{"radar_pdws_locate":radar_pdws})
#print(radar_pdws_located[0].erro)
return (radar_pdws_located,)
@app.cell
def _(lla2enu, np, stations):
def locate(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)):
stations_enu[i,:] = np.array(lla2enu(*stations[i].lla,*stations[0].lla))
for i in range(len(radar_pdws)):
doa = radar_pdws[i].doa.T
erro = np.empty(len(doa))
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)
# location_estimated[j,:] = np.array([lat,lon,0])
# radar_pdws[i].location = np.empty((len(radar_pdws[i].pri),3))
# radar_pdws[i].erro = np.empty(len(radar_pdws[i].pri))
# for j in range(len(radar_pdws[i].pri)):
# radar_pdws[i].location[j,:] = location_estimated[j,:]
# radar_pdws[i].erro[j] = erro[j]
return radar_pdws
def least_square(doa,e,n):
doa_rad = np.array(np.deg2rad(doa),ndmin=2)
l = doa_rad.size
ee = e[0:l]
nn = n[0:l]
fmtx = ee - nn * np.tan(doa_rad)
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
return least_square, locate
@app.cell
def _(np):
import pyproj
import scipy.spatial.transform
from scipy.spatial.transform import Rotation as R
from geopy.distance import geodesic
def lla2ecef(lat,lon,alt):
"""
将经纬度高度（LLA）转换为地心地固坐标系（ECEF）坐标。
@@ -369,64 +404,214 @@ def _():
x, y, z = transformer.transform(lat, lon, alt, radians=False)
return x, y, z
def lla2enu(lat, lon, alt, lat0, lon0, alt0):
def lla2enu(lat, lon, alt, lat_org, lon_org, alt_org):
"""
将经度、纬度和高度（LLA）坐标转换为东 - 北 - 天（ENU）坐标。
参数:
lon (float): 待转换点的经度（度）
lat (float): 待转换点的纬度（度）
alt (float): 待转换点的高度（米）
lon0 (float): 参考点的经度（度）
lat0 (float): 参考点的纬度（度）
alt0 (float): 参考点的高度（米）
返回:
tuple: 包含 E（东）、N（北）、U（天）坐标的元组（米）
将 LLA 坐标转换为 ENU 坐标
:param lat: 目标点的纬度（单位：度）
:param lon: 目标点的经度（单位：度）
:param alt: 目标点的高度（单位：米）
:param lat_org: 参考点的纬度（单位：度）
:param lon_org: 参考点的经度（单位：度）
:param alt_org: 参考点的高度（单位：米）
:return: 转换后的 ENU 坐标（东、北、天）
"""
# 定义 WGS84 经纬度坐标系统
lla = pyproj.CRS("EPSG:4326")
# 定义转换器
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'},
)
# 定义局部的 ENU 坐标系统
enu = pyproj.CRS.from_string(f"+proj=aeqd +lat_0={lat0} +lon_0={lon0} +x_0=0 +y_0=0 +datum=WGS84 +units=m +no_defs")
# 将目标点和参考点的 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)
# 创建坐标转换器
transformer = pyproj.Transformer.from_crs(lla, enu)
# 计算 ECEF 坐标差
ecef_delta = np.array([x - x_org, y - y_org, z - z_org])
# 执行坐标转换
e, n, u = transformer.transform(lon, lat, alt)
# 构造旋转矩阵
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)
u = u - alt0
# 将 ECEF 坐标差转换为 ENU 坐标
enu = rot_matrix.dot(ecef_delta)
return e, n, u
def ned2lla(ned,lla0):
pass
return lla2ecef, lla2enu, ned2lla, pyproj
return enu
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]
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]
def distance(lat1, lon1, lat2, lon2):
return geodesic((lat1, lon1), (lat2, lon2)).miles #.kilometers
# def haversine_distance(lat1, lon1, lat2, lon2):
# """
# 计算两个经纬度位置之间的距离（单位：千米）。
# :param lat1: 第一个点的纬度，单位：度
# :param lon1: 第一个点的经度，单位：度
# :param lat2: 第二个点的纬度，单位：度
# :param lon2: 第二个点的经度，单位：度
# :return: 两点之间的距离，单位：千米
# """
# # 地球的平均半径，单位：千米
# R = 6371.0
# # 将经纬度从度转换为弧度
# lat1_rad = math.radians(lat1)
# lon1_rad = math.radians(lon1)
# lat2_rad = math.radians(lat2)
# lon2_rad = math.radians(lon2)
# # 计算纬度和经度的差值
# dlat = lat2_rad - lat1_rad
# dlon = lon2_rad - lon1_rad
# # 应用 Haversine 公式
# a = math.sin(dlat / 2)**2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlon / 2)**2
# c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
# # 计算距离
# distance = R * c
# return distance
# def haversine_distance(lat1, lon1, lat2, lon2):
# """
# 计算两个经纬度点之间的距离（单位：米）
# :param lat1: 第一个点的纬度
# :param lon1: 第一个点的经度
# :param lat2: 第二个点的纬度
# :param lon2: 第二个点的经度
# :return: 两点之间的距离（单位：米）
# """
# # 将经纬度从度转换为弧度
# lat1, lon1, lat2, lon2 = map(math.radians, [lat1, lon1, lat2, lon2])
# # Haversine 公式
# dlat = lat2 - lat1
# dlon = lon2 - lon1
# a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
# c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
# # 地球平均半径（单位：米）
# R = 6371000
# distance = R * c
# return distance
# def haversine_distance(lat1, lon1, lat2, lon2):
# # Convert latitude and longitude from degrees to radians
# lat1, lon1, lat2, lon2 = map(math.radians, [lat1, lon1, lat2, lon2])
# # Haversine formula
# dlat = lat2 - lat1
# dlon = lon2 - lon1
# a = math.sin(dlat / 2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2)**2
# c = 2 * math.asin(math.sqrt(a))
# # Radius of Earth in kilometers. Use 3956 for miles
# r = 6371
# # Calculate the result
# distance = c * r
# return
return (
R,
distance,
enu2lla,
geodesic,
lla2ecef,
lla2enu,
lla2ned,
ned2lla,
pyproj,
scipy,
)
@app.cell
def _(lla2enu):
def _(distance, 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}) 米")
# 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")
# ans =
# 转换为 LLA 坐标
#enu2lla([-7134.8, -4556.3, 2852.4], [46.017, 7.750, 1673],"ellipsoid")
lla = enu2lla(-7134.8, -4556.3, 2852.4, 46.017, 7.750, 1673) #ok
print("enu2lla：", lla)
# 1.0e+05 *
#后两个实现和matlab对不上
# 转换为NED坐标
#lla2ned([45.976, 7.658, 4531], [46.017, 7.750, 1673],"ellipsoid")
n, e, d = lla2ned(45.976, 7.658, 4531, 46.017, 7.750, 1673) #ok
print(f"lla2ned：n={n:.2f} m, e ={e:.2f} m, d={d:.2f} m")
# 转换为LLA坐标
#ned2lla([-4556.3, -7134.8, -2852.4], [46.017, 7.750, 1673],"ellipsoid")
lat, lon, alt = ned2lla(-4556.3, -7134.8, -2852.4, 46.017, 7.750, 1673) #ok
print(f"ned2lla：lat={lat:.6f}°, lon={lon:.6f}°, alt={alt:.2f}")
# 0.9648 1.1125 -0.0163
e, n, u = lla2enu(30.0,120.0, 120.0, 29.0,119.0, 50.0)
print(f"ENU 坐标: E = {e} m, N = {n} m, U = {u} m")
return e, n, u
#distance(34.0522, -118.2437, 40.7128, -74.0060)
print(distance(34.0522, -118.2437, 40.7128, -74.0060))
return X, Y, Z, alt, d, e, lat, lla, lon, n, u
@app.cell
def _(np):
a = np.array([1,2],ndmin = 2).transpose()
b = np.array([3,4],ndmin = 2).transpose()
np.hstack((a,b))
def _():
np.empty((2,3))
return a, b
return
if __name__ == "__main__":