46 lines
1.8 KiB
Python
46 lines
1.8 KiB
Python
|
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
|