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

allow to set static factor

Browse source
This commit is contained in:
Laura Klünder 2023-11-11 16:34:55 +01:00
parent 0a9bf327c3
commit 94af40d2f1
1 changed files with 8 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -90,9 +90,14 @@ class RangeLocator:
# TODO: three points aren't really enough for precise results? hm. maybe just a 2d fix then?
pass
factor = None
# rating the guess by calculating the distances
def rate_guess(guess):
return scipy.linalg.norm(np_ranges[:, :3]-guess[:3], axis=1)*guess[3]-np_ranges[:, 3]
diffs = scipy.linalg.norm(np_ranges[:, :3] - guess[:3], axis=1) * (factor or guess[3]) - np_ranges[:, 3]
#if (diffs < -200).any():
# return diffs+10000-np.clip(diffs, None, -200)*10
return diffs
# initial guess i the average of all beacons, with scale 1
initial_guess = np.append(np.average(np_ranges[:, :3], axis=0), 1)
@ -112,8 +117,8 @@ class RangeLocator:
)
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[:, :3] - results.x[:3], axis=1) * results.x[3])))
print("result ranges:", ", ".join(("%.2f" % i) for i in tuple(scipy.linalg.norm(np_ranges[:, :3] - results.x[:3], axis=1)*(factor or results.x[3]))))
print("height:", results.x[2])
print("scale:", results.x[3])
print("scale:", (factor or results.x[3]))
return location