from UWB.across_detect import * from UWB.consensus import * class PositionFixer: def __init__(self, anchor_coord) -> None: self.anchor_coord = anchor_coord self.position_data = {} self.max_track_time = 0.7 def get_position(self, position_mes): tag = position_mes[TAG] tag_detect_mes = position_mes self.position_data.setdefault(tag, []) self.position_data[tag].append(tag_detect_mes) def get_record(self, tag): this_time = self.position_data[tag][-1][RECORD] record = self.position_data[tag] fitness_record = list(filter(lambda x: this_time - x[RECORD] < self.max_track_time, record)) self.position_data[tag] = fitness_record merge_position_data = {} for one in fitness_record: dist_mes = one[DIST_MES] merge_position_data.update(dist_mes) merge_position_data = {anchor: value for anchor, value in merge_position_data.items()} return merge_position_data def update_position(self, merge_position_data): if len(merge_position_data) < 3: return None anchor_coord_list = [self.anchor_coord[anchor] for anchor in merge_position_data.keys()] distance_mes = list(merge_position_data.values()) if len(merge_position_data) > 3: estimate_pos = get_tag_pos_from_anchor(anchor_coord_list, distance_mes) elif 2 < len(merge_position_data) <= 3: estimate_pos = trilateration( x1=anchor_coord_list[0][0], y1=anchor_coord_list[0][1], d1=distance_mes[0], x2=anchor_coord_list[1][0], y2=anchor_coord_list[1][1], d2=distance_mes[1], x3=anchor_coord_list[2][0], y3=anchor_coord_list[2][1], d3=distance_mes[2], ) else: return None return estimate_pos