初始化项目

agent-common/SplitProject/ranjing-python-devfusion/SensorTool.py

@@ -0,0 +1,142 @@
+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