more rangelocator experimenting

src/c3nav/mesh/consumers.py

@@ -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):

src/c3nav/routing/rangelocator.py

@@ -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
+            result = np.abs(diff_func(guess))
-            return factors - np.mean(factors)
+            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,
+            x=result_pos[0]/100,
-            y=results.x[1]/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