142 lines
4.4 KiB
Python
142 lines
4.4 KiB
Python
import numpy as np
|
||
from scipy import signal
|
||
|
||
|
||
class AoAConverter:
|
||
def __init__(self):
|
||
self.p = [1e8, 1e8, 1e8]
|
||
|
||
def to_cartesian(self, theta_rad, phi_rad):
|
||
# theta_rad = np.radians(theta)
|
||
# phi_rad = np.radians(phi)
|
||
# 注意！程序输入的是弧度单位
|
||
"""将球坐标转换为直角坐标"""
|
||
x = np.sin(theta_rad) * np.cos(phi_rad)
|
||
y = np.sin(theta_rad) * np.sin(phi_rad)
|
||
z = np.cos(theta_rad)
|
||
pc =np.array([x,y,z])
|
||
return pc
|
||
|
||
def calc_error(self, pc, mc):
|
||
# 计算预测坐标与实际观测坐标之间的差的平方
|
||
mc = np.expand_dims(mc, axis=1)
|
||
diff_squared = (pc - mc) ** 2
|
||
# 对差值的平方求和，得到误差的平方
|
||
error_squared = np.sum(diff_squared, axis=0)
|
||
# 开平方根得到误差
|
||
return np.sqrt(error_squared)
|
||
|
||
import numpy as np
|
||
|
||
def find_best_r(self, theta, phi, mc, r_range):
|
||
"""在给定范围内搜索最优的 r 值"""
|
||
# 将 r_range 转换为 NumPy 数组，以便进行矢量化操作
|
||
r_values = np.array(r_range)
|
||
# 计算所有可能的直角坐标
|
||
pc = self.to_cartesian(theta, phi)
|
||
# 进行维度扩充以进行矩阵乘法
|
||
r_values = np.expand_dims(r_values, axis=0)
|
||
pc = np.expand_dims(pc, axis=1)
|
||
# 计算所有 r 值对应的误差
|
||
# print([pc.shape,r_values.shape])
|
||
D = np.dot(pc, r_values)
|
||
errors = self.calc_error(D, mc)
|
||
r_values = np.squeeze(r_values)
|
||
|
||
# 找到最小误差及其对应的 r 值
|
||
min_error = np.min(errors)
|
||
best_r = r_values[np.argmin(errors)] #因为多加了一维，所以这里要反求0
|
||
|
||
return [best_r,min_error]
|
||
|
||
def projected_measure(self,theta, phi, r,p0):
|
||
pc = self.to_cartesian(theta, phi)
|
||
neo_p = r*pc + p0
|
||
return np.array(neo_p)
|
||
|
||
converter = AoAConverter()
|
||
|
||
|
||
def calculate_euclidean_distances(A, BX):
|
||
# 计算A和B之间的欧式距离
|
||
B = BX['data_matrix']
|
||
N = B.shape[0]
|
||
r_range = np.linspace(-5, 5, 100)
|
||
if BX.get('AOA_pos'):
|
||
# 若是来自AOA的数据，则进行替换
|
||
sensor_pos = BX.get('AOA_pos')
|
||
ob_pos = A - sensor_pos
|
||
r0 = np.linalg.norm(ob_pos)
|
||
B_new = []
|
||
for i in range(N):
|
||
theta = B[i,0]
|
||
phi = B[i,1]
|
||
[best_r,min_error] = converter.find_best_r(theta, phi,ob_pos, r0+r_range)
|
||
print(min_error)
|
||
B_new.append(converter.projected_measure(theta, phi,best_r,sensor_pos))
|
||
B_new = np.array(B_new)
|
||
else:
|
||
B_new = B
|
||
|
||
|
||
distances = np.linalg.norm(A - B_new, axis=1)
|
||
# 找到最小距离及其索引
|
||
min_distance_index = np.argmin(distances)
|
||
min_distance = distances[min_distance_index]
|
||
return [min_distance, min_distance_index, B_new]
|
||
|
||
def are_lists_equal(listA, listB):
|
||
# 对两个列表中的子列表进行排序
|
||
if len(listA) == 0:
|
||
return False
|
||
sorted_listA = sorted(listA, key=lambda x: (x[0], x[1]))
|
||
sorted_listB = sorted(listB, key=lambda x: (x[0], x[1]))
|
||
# 比较排序后的列表是否相等
|
||
return sorted_listA == sorted_listB
|
||
|
||
def sigmoid(x, a=10, b=0.1):
|
||
# 调整Sigmoid函数使其在x=4时值为0.5
|
||
# a和b是调整参数，用于控制函数的形状
|
||
return 1 / (1 + np.exp(-a * (x - 1))) + b
|
||
|
||
|
||
def calculate_correlation(A, B):
|
||
"""
|
||
计算两个数组矩阵所有列的相关系数的最大值。
|
||
|
||
参数:
|
||
A -- 第一个NumPy数组
|
||
B -- 第二个NumPy数组
|
||
"""
|
||
A = np.exp(-1j*A/50)
|
||
B = np.exp(1j*B/50)
|
||
corr_res = []
|
||
for col in range(3):
|
||
a = A[:, col]
|
||
b = B[:, col]
|
||
convolution = signal.convolve(a, b[::-1])
|
||
corr_res.append(convolution)
|
||
max_corr = np.sum(np.abs(np.array(corr_res)),0)
|
||
max_corr = np.max(max_corr)/3
|
||
|
||
return max_corr
|
||
|
||
|
||
def calculate_history_distances(target, b):
|
||
# 使用前后向的形式进行计算
|
||
A = target.m_history
|
||
v = target.v
|
||
# 计算每一行与向量b的差的L2范数（欧氏距离）
|
||
if A.shape[0] < 10:
|
||
return np.inf
|
||
local_time = np.linspace(0, 10, 20)
|
||
local_time = np.expand_dims(local_time, axis=1)
|
||
v = np.expand_dims(v, axis=1)
|
||
A_pre = A[-10:,0:3]
|
||
A_post = np.dot(local_time,v.T)
|
||
A_all = np.vstack((A_pre, A_post))
|
||
distances = np.linalg.norm(A_all - b, axis=1)
|
||
# 找到最小距离
|
||
min_distance = np.min(distances)
|
||
|
||
return min_distance |