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

update range locate code with new gwen(tm) algorithm

Browse source
This commit is contained in:
Laura Klünder 2023-11-11 17:19:16 +01:00
parent 4dd79c238f
commit c3a9b178f7
1 changed files with 19 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1,6 +1,7 @@
import pickle import pickle
import threading import threading
from dataclasses import dataclass from dataclasses import dataclass
from pprint import pprint
from typing import Self from typing import Self
import numpy as np import numpy as np
@ -63,48 +64,45 @@ class RangeLocator:
cls.cached = cls.load_nocache(update) cls.cached = cls.load_nocache(update)
return cls.cached return cls.cached
def locate(self, scan: dict[str, int], permissions=None): def locate(self, ranges: dict[str, int], permissions=None):
# get the i and peer for every peer that we actually know # get the i and peer for every peer that we actually know
ranges = tuple( relevant_ranges = tuple(
(i, peer) for i, peer in ( (i, distance) for i, distance in (
(self.beacon_lookup.get(bssid, None), distance) for bssid, distance in scan.items() (self.beacon_lookup.get(bssid, None), distance) for bssid, distance in ranges.items()
) if i is not None ) if i is not None
) )
# create 2d array with x, y, z, distance as rows # create 2d array with x, y, z, distance as rows
np_ranges = np.hstack(( np_ranges = np.hstack((
self.beacon_positions[tuple(i for i, distance in ranges), :], self.beacon_positions[tuple(i for i, distance in relevant_ranges), :],
np.array(tuple(distance for i, distance in ranges)).reshape((-1, 1)), np.array(tuple(distance for i, distance in relevant_ranges)).reshape((-1, 1)),
)) ))
if np_ranges.shape[0] < 3: if np_ranges.shape[0] < 3:
# can't get a good result from just two beacons # can't get a good result from just two beacons
# todo: maybe we can at least give… something? # todo: maybe we can at least give… something?
print('less than 3 ranges, can\'t do ranging')
return None return None
if np_ranges.shape[0] == 3: if np_ranges.shape[0] == 3:
# TODO: three points aren't really enough for precise results? hm. maybe just a 2d fix then? print('2D trilateration')
pass dimensions = 2
else:
dimensions = 2 print('3D trilateration')
dimensions = 3
measured_ranges = np_ranges[:, 3] measured_ranges = np_ranges[:, 3]
measured_ranges = measured_ranges / np.max(measured_ranges)
# rating the guess by calculating the distances # rating the guess by calculating the distances
def rate_guess(guess): def cost_func(guess):
guessed_ranges = scipy.linalg.norm(np_ranges[:, :dimensions] - guess[:dimensions], axis=1) factors = scipy.linalg.norm(np_ranges[:, :dimensions] - guess[:dimensions], axis=1) / measured_ranges
guessed_ranges /= np.max(guessed_ranges) return factors - np.mean(factors)
diffs = guessed_ranges-measured_ranges
if (diffs < -200).any():
return diffs+100-np.clip(diffs, None, -200)*10
return diffs
# initial guess i the average of all beacons, with scale 1 # initial guess i the average of all beacons, with scale 1
initial_guess = np.average(np_ranges[:, :dimensions], axis=0) initial_guess = np.average(np_ranges[:, :dimensions], axis=0)
# here the magic happens # here the magic happens
results = least_squares(rate_guess, initial_guess) results = least_squares(cost_func, initial_guess)
# create result # create result
# todo: figure out level # todo: figure out level
@ -117,10 +115,12 @@ class RangeLocator:
icon='my_location' icon='my_location'
) )
pprint(relevant_ranges)
print("measured ranges:", ", ".join(("%.2f" % i) for i in tuple(np_ranges[:, 3]))) print("measured ranges:", ", ".join(("%.2f" % i) for i in tuple(np_ranges[:, 3])))
print("result ranges:", ", ".join( 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] - results.x[:dimensions], axis=1))
)) ))
print("cost:", cost_func(results.x))
if dimensions > 2: if dimensions > 2:
print("height:", results.x[2]) print("height:", results.x[2])
# print("scale:", (factor or results.x[3])) # print("scale:", (factor or results.x[3]))