2025-summer/team-3
12
Fork 0
Code Issues Pull requests Projects Releases Activity

more rangelocator experimenting

Browse source
This commit is contained in:
Laura Klünder 2023-11-27 22:09:31 +01:00
parent d3c4d71331
commit ad98eecf41
2 changed files with 81 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/c3nav/mesh/consumers.py
View file

@ -638,12 +638,12 @@ class MeshUIConsumer(AsyncJsonWebsocketConsumer):
return
if data["msg"]["msg_type"] == MeshMessageType.LOCATE_RANGE_RESULTS.name:
data = data.copy()
location = await self.locator(data["msg"])
location = await self.locator(data["msg"], data["msg"]["src"])
data["position"] = None if not location else (int(location.x*100), int(location.y*100), int(location.z*100))
await self.send_json(data)
@database_sync_to_async
def locator(self, msg):
def locator(self, msg, orig_addr=None):
locator = RangeLocator.load()
return locator.locate(
{
@ -651,7 +651,8 @@ class MeshUIConsumer(AsyncJsonWebsocketConsumer):
for r in msg["ranges"]
if r["distance"] != 0xFFFF
},
None
permissions=None,
orig_addr=orig_addr,
)
async def disconnect(self, code):

90
src/c3nav/routing/rangelocator.py
View file

@ -77,7 +77,8 @@ class RangeLocator:
cls.cached = cls.load_nocache(update)
return cls.cached
def locate(self, ranges: dict[str, int], permissions=None):
def locate(self, ranges: dict[str, int], permissions=None, orig_addr=None):
pprint(ranges)
# get the i and peer for every peer that we actually know
relevant_ranges = tuple(
(i, distance) for i, distance in (
@ -85,19 +86,25 @@ class RangeLocator:
) if i is not None
)
relevant_positions = self.beacon_positions[tuple(i for i, _ in relevant_ranges), :]
mean = np.mean(relevant_positions, axis=0)
relevant_positions = relevant_positions
# create 2d array with x, y, z, distance as rows
np_ranges = np.hstack((
self.beacon_positions[tuple(i for i, distance in relevant_ranges), :],
relevant_positions,
np.array(tuple(distance for i, distance in relevant_ranges)).reshape((-1, 1)),
))
print(np_ranges)
if np_ranges.shape[0] < 3:
# can't get a good result from just two beacons
# todo: maybe we can at least give… something?
print('less than 3 ranges, can\'t do ranging')
return None
if np_ranges.shape[0] == 3:
if np_ranges.shape[0] == 3 and 0:
print('2D trilateration')
dimensions = 2
else:
@ -105,38 +112,95 @@ class RangeLocator:
dimensions = 3
measured_ranges = np_ranges[:, 3]
print('a', measured_ranges)
#measured_ranges[measured_ranges<1] = 1
print('b', measured_ranges)
# rating the guess by calculating the distances
def diff_func(guess):
result = scipy.linalg.norm(np_ranges[:, :dimensions] - guess[:dimensions], axis=1) - measured_ranges
#print(result)
return result
# factors = scipy.linalg.norm(np_ranges[:, :dimensions] - guess[:dimensions], axis=1) / measured_ranges
# return factors - np.mean(factors)
def cost_func(guess):
factors = scipy.linalg.norm(np_ranges[:, :dimensions] - guess[:dimensions], axis=1) / measured_ranges
return factors - np.mean(factors)
result = np.abs(diff_func(guess))
result[result<300] = result[result<300]/3+200
return result
# initial guess i the average of all beacons, with scale 1
initial_guess = np.average(np_ranges[:, :dimensions], axis=0)
# here the magic happens
results = least_squares(cost_func, initial_guess)
results = least_squares(
fun=cost_func,
#jac="3-point",
loss="linear",
bounds=(
np.min(self.beacon_positions[:, :dimensions], axis=0) - np.array([200, 200, 100])[:dimensions],
np.max(self.beacon_positions[:, :dimensions], axis=0) + np.array([200, 200, 100])[:dimensions],
),
x0=initial_guess,
)
# create result
# todo: figure out level
result_pos = results.x
from c3nav.mapdata.models import Level
location = CustomLocation(
level=Level.objects.first(),
x=results.x[0]/100,
y=results.x[1]/100,
x=result_pos[0]/100,
y=result_pos[1]/100,
permissions=(),
icon='my_location'
)#
location.z = results.x[2]
)
location.z = result_pos[2]/100
pprint(relevant_ranges)
orig_xyz = None
print('orig_addr', orig_addr)
if orig_addr:
orig_xyz = self.get_xyz(orig_addr)
if orig_xyz:
orig_xyz = np.array(orig_xyz)
print()
print("result:", ", ".join(("%.2f" % i) for i in tuple(result_pos)))
if orig_xyz is not None:
print("correct:", ", ".join(("%.2f" % i) for i in tuple(orig_xyz)))
print("diff:", ", ".join(("%.2f" % i) for i in tuple(orig_xyz-result_pos)))
print()
print("measured ranges:", ", ".join(("%.2f" % i) for i in tuple(np_ranges[:, 3])))
print("result ranges:", ", ".join(
("%.2f" % i) for i in tuple(scipy.linalg.norm(np_ranges[:, :dimensions] - results.x[:dimensions], axis=1))
("%.2f" % i) for i in tuple(scipy.linalg.norm(np_ranges[:, :dimensions] - result_pos[:dimensions], axis=1))
))
print("cost:", cost_func(results.x))
if orig_xyz is not None:
print("correct ranges:", ", ".join(
("%.2f" % i) for i in tuple(scipy.linalg.norm(np_ranges[:, :dimensions] - orig_xyz[:dimensions], axis=1))
))
print()
print("diff result-measured:", ", ".join(
("%.2f" % i) for i in
tuple(diff_func(result_pos))
))
if orig_xyz is not None:
print("diff correct-measured:", ", ".join(
("%.2f" % i) for i in
tuple(diff_func(orig_xyz))
))
def print_cost(title, pos):
cost = cost_func(pos)
print(title, ", ".join(
("%.2f" % i) for i in cost
), '=', np.sum(cost**2))
print_cost("cost:", result_pos)
if orig_xyz is not None:
print_cost("cost of correct position:", orig_xyz)
if dimensions > 2:
print("height:", results.x[2])
print("height:", result_pos[2])
# print("scale:", (factor or results.x[3]))
return location