import datetime from os import error import numpy as np from config import * def calculate_euclidean_distances(A, B): # 计算A和B之间的欧式距离 distances = np.linalg.norm(A - B, axis=1) # 找到最小距离及其索引 min_distance_index = np.argmin(distances) min_distance = distances[min_distance_index] return min_distance, min_distance_index 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=1时值为0.5 # a和b是调整参数，用于控制函数的形状 return 1 / (1 + np.exp(-a * (x - shift_value))) + b class KalmanFilter: def __init__(self, measurement, com_id, measurement_variance=1,process_variance=1e-1): current_time = datetime.datetime.now() timestamp = int(current_time.timestamp() * 1000000) ms = measurement.tolist() self.m = np.array([ms[0],ms[1],ms[2],0,0,0]) # 状态量,6维度 self.origin = [com_id] #origin 表示最强响应 self.source = self.origin #source 表示所有关联的观测值 self.survive = np.array(survive_initial) # 初始化生存值 self.duration = 0 self.counter = 0 self.id = str(timestamp % 3600000000 + np.random.randint(1000)) self.F = [[1,0,0,1,0,0], [0,1,0,0,1,0], [0,0,1,0,0,1], [0,0,0,1,0,0], [0,0,0,0,1,0], [0,0,0,0,0,1]] self.F = np.array(self.F) self.H = [[1,0,0,0,0,0], [0,1,0,0,0,0], [0,0,1,0,0,0]] self.H = np.array(self.H) self.R = measurement_variance * np.eye(3) self.Q = process_variance * np.eye(6) self.Q[3, 3] = self.Q[3, 3] * 1e-3 self.Q[4, 4] = self.Q[4, 4] * 1e-3 self.Q[5, 5] = self.Q[5, 5] * 1e-3 self.P = np.eye(6)*0.1 self.I = np.eye(6) self.expend = 1 self.v = np.array([0,0,0]) self.born_time = int(current_time.timestamp() * 1000) self.latest_update = self.born_time self.m_history = self.m self.s_history = [] self.origin_set = [self.origin] def predict(self): F = self.F self.m = np.dot(F,self.m.T) # 简单一步预测模型 self.m = self.m.T self.P = np.dot(np.dot(F,self.P),F.T) + self.Q self.survive = self.survive * decay # 应用衰减值 self.origin_set = np.unique(np.array(self.origin_set), axis=0).tolist() # 计算关联集合 def update(self, res, run_timestamp, gate): self.duration += 0.6 # 每次更新时，持续时间+0.6 if len(res['distances']) == 0: mmd = 1e8 else: min_distance_index = np.argmin(res['distances']) mmd = res['distances'][min_distance_index] measurement = res['measurements'][min_distance_index] # 进行更新 if mmd < gate * self.expend: H = self.H I = self.I self.expend = max(self.expend * 0.8, 1) kalman_gain = np.dot(np.dot(self.P,H.T),np.linalg.pinv(np.dot(np.dot(H,self.P),H.T)+self.R)) self.m += np.dot(kalman_gain,(measurement.T - np.dot(H,self.m.T))) self.m = self.m.T self.P = np.dot((I - np.dot(kalman_gain,H)),self.P) self.origin = [res['key_ids'][min_distance_index]] self.counter += 1 self.survive = sigmoid(self.counter) # 新映射函数 # 如下操作防止对速度过于自信 self.P[3, 3] = max(1e-1, self.P[3, 3]) self.P[4, 4] = max(1e-1, self.P[4, 4]) self.P[5, 5] = max(1e-1, self.P[5, 5]) # 截取速度 self.v = self.m[3:6] self.origin_set.append(self.origin) self.latest_update = run_timestamp #对时间进行处理 else: self.expend = min(self.expend*1.2,1.5) # 若关联不上，则扩大门限继续搜索 self.P[3, 3] = min(self.P[3, 3]*1.1,1) self.P[4, 4] = min(self.P[4, 4]*1.1,1) self.P[5, 5] = min(self.P[5, 5]*1.1,1) self.counter -= 1 self.counter = max(self.counter,0) self.m_history = np.vstack((self.m_history, self.m)) self.s_history.append(self.survive) def one_correlation(self, data_matrix, id_list): # 计算现有数据与data_matrix的差距 min_distance, min_index = calculate_euclidean_distances(self.m[0:3], data_matrix) m_id = id_list[min_index] measurement = data_matrix[min_index, :] return m_id, min_distance, measurement def correlation(self, sensor_data): # 遍历传感器进行计算 res = {'m_ids':[], 'distances':[], 'measurements':[], 'key_ids':[]} for value in sensor_data: if len(value['id_list']) > 0: m_id, min_distance, measurement = self.one_correlation(value['data_matrix'], value['id_list']) key = value['deviceId'] res['m_ids'].append(m_id) res['measurements'].append(measurement) res['key_ids'].append([key, m_id]) # 将发生过关联的目标赋予更大的置信度 if [key, m_id] in self.origin_set: min_distance = min_distance * 0.2 res['distances'].append(min_distance) return res #融合类的构造函数 class DataFusion: def __init__(self,gate=25,interval = 1,fusion_type = 1, measuremrnt_variance=1,process_variance =1e-1): """ 初始化DataFusion类。 """ # self.task_id = task_id self.interval = interval self.gate = gate self.targets = [] self.fusion_type = fusion_type self.existence_thres = 0.01 self.show_thres = show_thres self.process_variance = process_variance self.measuremrnt_variance = measuremrnt_variance def set_parameter(self,fusion_parms): print("GO!!!!!!!!!") print(fusion_parms) def obtain_priority(self,sensor_data): self.priority_dict = dict() for data in sensor_data: if data.get('priority'): self.priority_dict[data['deviceId']] = data['priority'] else: self.priority_dict[data['deviceId']] = 1 def out_transformer(self,target): out_former = { 'objectId': target.id, 'survive': target.survive.tolist(), 'state': target.m.tolist(), 'speed': np.linalg.norm(target.v).tolist() / self.interval, 'source': target.source, 'sigma': np.diag(target.P).tolist(), 'X': target.m[0].tolist(), 'Y': target.m[1].tolist(), 'Z': target.m[2].tolist(), 'Vx': target.v[0].tolist(), 'Vy': target.v[1].tolist(), 'Vz': target.v[2].tolist(), 'born_time': str(target.born_time) } return out_former def run(self, sensor_data): current_time = datetime.datetime.now() run_timestamp = int(current_time.timestamp() * 1000) fusion_data = [] selected_list = [] self.obtain_priority(sensor_data) # 遍历所有已知对象 for target in self.targets: print(f"Fusion target id:{target.id} with survive: {target.survive} at :{target.m}\n") if target.survive < self.existence_thres: continue target.predict() res = target.correlation(sensor_data) target.update(res,run_timestamp,self.gate) # ================================================== now_id = [] t_sum = 0 for r, distance in enumerate(res['distances']): if distance < self.gate: now_id.append(res['key_ids'][r]) selected_list.append(res['key_ids'][r]) D_Id = res['key_ids'][r][0] t_sum += self.priority_dict[D_Id] target.source = now_id # ================================================== if self.fusion_type == 2 and t_sum < 2: target.survive = target.survive * 0.5 out_former = self.out_transformer(target) if target.survive > self.show_thres: # 若存活概率大于0.4，则写入数据文件 fusion_data.append(out_former) # 根据匹配关系筛选数值 self.selected_list = selected_list for data in sensor_data: self.new_born(data) self.remove_duplicates() # ================================================== self.fusion_process_log(fusion_data) return fusion_data def new_born(self,value,): for j, id in enumerate(value['id_list']): key = value['deviceId'] if [key, id] not in self.selected_list: if self.fusion_type == 3: if value['priority'] > 50: self.targets.append(KalmanFilter(value['data_matrix'][j, :], [key, id],self.measuremrnt_variance,self.process_variance)) else: self.targets.append(KalmanFilter(value['data_matrix'][j, :], [key, id],self.measuremrnt_variance,self.process_variance)) self.selected_list.append([key, id]) # 把新增的目标，加入到集合中去 def remove_duplicates(self): # 创建一个空列表用于存储需要删除的列表的索引 to_delete = [] # 遍历所有列表的索引 for i in range(len(self.targets)): if self.targets[i].survive < self.existence_thres: to_delete.append(self.targets[i].id) continue if self.targets[i].survive < self.show_thres: continue for j in range(i + 1, len(self.targets)): # 比较两个列表是否相同 if are_lists_equal(self.targets[i].source, self.targets[j].source): # 如果列表相同，记录编号较大的索引 if self.targets[i].duration < self.targets[j].duration: to_delete.append(self.targets[i].id) else: to_delete.append(self.targets[j].id) # 使用删除法，提高目标管理效率 for item_id in sorted(to_delete, reverse=True): for target in self.targets: if target.id == item_id: self.targets.remove(target) break def fusion_process_log(self,fusion_data): current_time = datetime.datetime.now() # 格式化时间为年月日时分秒格式 formatted_time = current_time.strftime('%Y-%m-%d %H:%M:%S') with open('process_log.txt', 'a') as log_file: # 以追加模式打开日志文件 log_file.write('=====================\n') # 写入分隔符 log_file.write(f"time: {formatted_time}\n") # 写入分隔符 log_file.write(f"data:\n {fusion_data}\n") # 写入消息内容