784 lines
27 KiB
Python
784 lines
27 KiB
Python
import marimo
|
||
|
||
__generated_with = "0.10.16"
|
||
app = marimo.App(width="medium")
|
||
|
||
|
||
@app.cell
|
||
def _():
|
||
import numpy as np
|
||
import math
|
||
import matplotlib.pyplot as plt
|
||
import marimo as mo
|
||
import scipy as sci
|
||
return math, mo, np, plt, sci
|
||
|
||
|
||
@app.cell
|
||
def _(lla2ecef, np):
|
||
class Station:
|
||
def __init__(self, lat, lon, alt):
|
||
self.lla = np.array([lat, lon, alt])
|
||
self.xyz = np.array(lla2ecef(*self.lla))
|
||
class Target:
|
||
def __init__(self, lat, lon, alt):
|
||
self.lla = np.array([lat, lon, alt])
|
||
self.xyz = np.array(lla2ecef(*self.lla))
|
||
class Radar:
|
||
def __init__(self, lat, lon, alt, /, *, pri_info, rf_info, pt, gt, pw):
|
||
self.lla = np.array([lat, lon, alt])
|
||
self.xyz = np.array(lla2ecef(*self.lla))
|
||
self.pri_info = pri_info
|
||
self.rf_info = rf_info
|
||
self.pt = pt
|
||
self.gt = gt
|
||
self.pw = pw
|
||
class RadarPdw:
|
||
def __init__(self):
|
||
pass
|
||
return Radar, RadarPdw, Station, Target
|
||
|
||
|
||
@app.cell
|
||
def _(Radar, Station, Target):
|
||
stations = [Station(31.2304, 121.4737, 0), Station(31.5200, 121.6400, 0)]
|
||
targets = [
|
||
Target(31.2550, 121.4890, 0), # 目标1 陆
|
||
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(30.9500, 121.6500, 0), # 目标6 海
|
||
Target(30.8500, 121.7200, 0), # 目标7 海
|
||
Target(30.7800, 121.8000, 0), # 目标8 海
|
||
Target(30.9200, 121.8500, 0), # 目标9 海
|
||
Target(30.6700, 121.9000, 0), # 目标10 海
|
||
]
|
||
radars = [
|
||
Radar(
|
||
31.2550,
|
||
121.4890,
|
||
0,
|
||
pri_info={"type": "STA", "values": [1000e-6, 1500e-6, 3000e-6]},
|
||
rf_info={"type": "Agility", "center": 1.1e9, "bw": 0.5e9},
|
||
pt=5.5e3,
|
||
gt=10**2.5,
|
||
pw=5e-6,
|
||
),
|
||
Radar(
|
||
31.2800,
|
||
121.5200,
|
||
0,
|
||
pri_info={"type": "STB", "values": [1732e-6]},
|
||
rf_info={"type": "STB", "center": 1.1e9, "bw": 0.0},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=3e-6,
|
||
),
|
||
Radar(
|
||
31.1900,
|
||
121.4800,
|
||
0,
|
||
pri_info={"type": "SLI", "values": [1800e-6, 3800e-6]},
|
||
rf_info={"type": "Agility", "center": 1.8e9, "bw": 0.6e9},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=2e-6,
|
||
),
|
||
Radar(
|
||
31.2700,
|
||
121.5500,
|
||
0,
|
||
pri_info={"type": "STB", "values": [2108e-6]},
|
||
rf_info={"type": "Agility", "center": 1.8e9, "bw": 0.6e9},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=4e-6,
|
||
),
|
||
# Radar(
|
||
# 31.2000,
|
||
# 121.6000,
|
||
# 0,
|
||
# pri_info={"type": "AGI", "values": [5500e-6, 4500e-6, 3500e-6]},
|
||
# rf_info={"type": "Agility", "center": 4e9, "bw": 0.6e9},
|
||
# pt=5e3,
|
||
# 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,
|
||
0,
|
||
pri_info={"type": "STB", "values": [1100e-6]},
|
||
rf_info={"type": "Agility", "center": 1.8e9, "bw": 0.6e9},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=3e-6,
|
||
),
|
||
Radar(
|
||
31.8500,
|
||
121.7200,
|
||
0,
|
||
pri_info={"type": "JTR", "values": [2489e-6]},
|
||
rf_info={"type": "Agility", "center": 1.8e9, "bw": 0.6e9},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=6e-6,
|
||
),
|
||
Radar(
|
||
31.7800,
|
||
121.8000,
|
||
0,
|
||
pri_info={"type": "STB", "values": [2763e-6]},
|
||
rf_info={"type": "Agility", "center": 3e9, "bw": 0.6e9},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=9e-6,
|
||
),
|
||
Radar(
|
||
30.9200,
|
||
121.8500,
|
||
0,
|
||
pri_info={"type": "STB", "values": [1377e-6]},
|
||
rf_info={"type": "Agility", "center": 1.8e9, "bw": 0.6e9},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=3e-6,
|
||
),
|
||
Radar(
|
||
30.6700,
|
||
121.9000,
|
||
0,
|
||
pri_info={"type": "JTR", "values": [2918e-6]},
|
||
rf_info={"type": "Agility", "center": 5.1e9, "bw": 0.6e9},
|
||
pt=5e3,
|
||
gt=1e3,
|
||
pw=1e-6,
|
||
),
|
||
]
|
||
assert(len(targets) == len(radars))
|
||
return radars, stations, targets
|
||
|
||
|
||
@app.cell
|
||
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
|
||
gr = 1 # 增益
|
||
radar_pdws = [RadarPdw() for i in range(len(radars))]
|
||
for i,radar in enumerate(radars):
|
||
# print(radar.xyz,stations[0].xyz)
|
||
propagation_time = calc_propagation_time(radar.xyz,stations[0].xyz)
|
||
toa,radar_pdws[i].pri = generate_pri(radar.pri_info,time)
|
||
radar_pdws[i].toa = toa + propagation_time
|
||
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 = 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
|
||
#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].rf = generate_rf(radar.rf_info,pulse_num)
|
||
radar_pdws[i].pt = radar.pt + radar.pt * np.random.normal(size=pulse_num) * 0.001
|
||
r = np.linalg.norm(radar.xyz-stations[0].xyz)
|
||
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 #此处从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]
|
||
x2 = pos2[0]
|
||
y2 = pos2[1]
|
||
delta_x = x2 - x1
|
||
delta_y = y2 - y1
|
||
theta_x = math.atan2(delta_y,delta_x) * (180/math.pi)
|
||
theta_n = 90 - theta_x
|
||
if theta_n < 0:
|
||
theta_n = theta_n + 360
|
||
return theta_n
|
||
|
||
def generate_rf(rf_info,num):
|
||
if rf_info["type"] == "STB": #固定
|
||
return np.full(num,rf_info["center"])
|
||
elif rf_info["type"] == "Agility": #捷变
|
||
rf = np.empty(num)
|
||
for i in range(num):
|
||
rf[i] = rf_info["center"] + rf_info["bw"] * (2 * np.random.rand() - 1)
|
||
return rf
|
||
else:
|
||
raise ValueError("未知的rf类型")
|
||
|
||
def generate_pri(pri_info,time):
|
||
if pri_info["type"] == "STB": #固定stable
|
||
pri_value = pri_info["values"][0]
|
||
n = round(time/pri_value)
|
||
pri = np.full(n,pri_value)
|
||
toa = np.cumsum(pri)
|
||
return toa,pri
|
||
|
||
elif pri_info["type"] == "JTR": #抖动jittered
|
||
pri_value = pri_info["values"][0]
|
||
n = round(time/pri_value)
|
||
m = 10
|
||
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, 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"]
|
||
pri_avg = np.mean(pri_values)
|
||
n = round(time/pri_avg)
|
||
m = len(pri_values)
|
||
pri = np.tile(pri_values,math.ceil(n/m))
|
||
pri = pri[0:n]
|
||
toa = np.cumsum(pri)
|
||
return toa,pri
|
||
|
||
elif pri_info["type"] == "SLI": #滑变Sliding
|
||
pri_values = pri_info["values"]
|
||
pri_avg = np.mean(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.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
|
||
else:
|
||
raise ValueError("未知的pri类型")
|
||
return (
|
||
calc_doa_north,
|
||
calc_propagation_time,
|
||
generate_pdw,
|
||
generate_pri,
|
||
generate_rf,
|
||
)
|
||
|
||
|
||
@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 _(distance, lla2enu, ned2lla, np, stations, targets):
|
||
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
|
||
from math import radians, degrees, sin, cos, atan2, sqrt, acos
|
||
|
||
def lla2ecef(lat,lon,alt):
|
||
"""
|
||
将经纬度高度（LLA）转换为地心地固坐标系（ECEF）坐标。
|
||
|
||
参数:
|
||
lon (float): 经度，单位为度。
|
||
lat (float): 纬度，单位为度。
|
||
alt (float): 海拔，单位为米。
|
||
|
||
返回:
|
||
tuple: 包含 ECEF 坐标 (x, y, z) 的元组，单位为米。
|
||
"""
|
||
# 定义源坐标系统（WGS84 经纬度）
|
||
lla = pyproj.CRS("EPSG:4326")
|
||
# 定义目标坐标系统（ECEF）
|
||
ecef = pyproj.CRS("EPSG:4978")
|
||
# 创建转换器对象
|
||
transformer = pyproj.Transformer.from_crs(lla, ecef)
|
||
# 进行坐标转换
|
||
x, y, z = transformer.transform(lat, lon, alt, radians=False)
|
||
return x, y, z
|
||
|
||
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: 点的海拔 (m)
|
||
:param ref_lat: 参考点纬度 (°)
|
||
:param ref_lon: 参考点经度 (°)
|
||
:param ref_alt: 参考点海拔 (m)
|
||
:return: ENU 坐标 (East, North, Up)
|
||
"""
|
||
# 将参考点和目标点的 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
|
||
|
||
def enu2lla(e, n, u, ref_lat, ref_lon, ref_alt):
|
||
"""
|
||
将 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)
|
||
"""
|
||
# 将参考点的 LLA 转为 ECEF 坐标
|
||
ref_ecef = lla2ecef(ref_lat, ref_lon, ref_alt)
|
||
|
||
# 构建 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)]
|
||
])
|
||
|
||
# 计算 ECEF 坐标
|
||
delta = np.linalg.inv(t) @ np.array([e, n, u])
|
||
point_ecef = ref_ecef + delta
|
||
|
||
# 将 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'},
|
||
# )
|
||
|
||
# # 将目标点和参考点的 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]
|
||
|
||
|
||
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 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):
|
||
# 将经纬度从度转换为弧度
|
||
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
|
||
|
||
# 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
|
||
|
||
# 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 distance
|
||
return (
|
||
R,
|
||
acos,
|
||
atan2,
|
||
cos,
|
||
degrees,
|
||
distance,
|
||
ecef2lla,
|
||
enu2lla,
|
||
geodesic,
|
||
lla2ecef,
|
||
lla2enu,
|
||
lla2ned,
|
||
ned2lla,
|
||
pyproj,
|
||
radians,
|
||
scipy,
|
||
sin,
|
||
sqrt,
|
||
)
|
||
|
||
|
||
@app.cell
|
||
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")
|
||
|
||
# 转换为 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)
|
||
|
||
#后两个实现和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}")
|
||
|
||
#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
|
||
|
||
|
||
if __name__ == "__main__":
|
||
app.run()
|