updated_cli

Wireless_Communication/UWB/Beacons_tag_position/realtime_location_cli_only.py

@@ -70,6 +70,7 @@ class AnchorTagCLI:
         self.tag2_distances = {"A1": 0.0, "A2": 0.0, "A3": 0.0, "A4": 0.0}
 
         self.anchors = {}
+        self.anchorHeight = 250
         self.anchors_coords_known = False
 
         if self.testing:
@@ -86,25 +87,30 @@ class AnchorTagCLI:
     def determine_anchor_coords(self):
         try:
             measured_distances = np.array(self.measured_distances)
-            noise_level = 0.0
-            measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances))
+            y_A = measured_distances[2]  # Distance from A to D
 
-            # Variables: x_B, y_B, x_C, y_C, y_A
-            initial_guess = [120, 100, 150, 200, 50] # [x_B, y_B, x_C, y_C, y_A]
-            maxBounds = 30000
-            bounds = ([0, 0, 0, 0, 0], [maxBounds] * 5)
+            # Guess for B based on distance A-B and B-D
+            x_B = measured_distances[0] / 2
+            y_B = measured_distances[4] / 2 + 200  # Start y_B above y_C
+
+            # Guess for C with symmetrical logic
+            x_C = -measured_distances[5] / 2  # Allow for negative x_C
+            y_C = -measured_distances[1] / 2  # Allow for negative y_C
+
+            initial_guess = [x_B, y_B, x_C, y_C, y_A]
+
+            min_dist = min(measured_distances)
+            max_dist = max(measured_distances)
+            lower_bounds = [-max_dist, -max_dist, -max_dist, -max_dist, min_dist / 2]
+            upper_bounds = [max_dist * 1.5 for i in range(5)]
 
             def error_function(variables, measured):
                 x_B, y_B, x_C, y_C, y_A = variables
-                # measured: [A, E, D, B, F, C]
-                a_measured = measured[0]
-                e_measured = measured[1]
-                d_measured = measured[2]
-                b_measured = measured[3]
-                f_measured = measured[4]
-                c_measured = measured[5]
-
-                # A=(0,y_A), B=(x_B,y_B), C=(x_C,y_C), D=(0,0)
+
+                # Map measured distances to a, e, d, b, f, c
+                a_measured, e_measured, d_measured, b_measured, f_measured, c_measured = measured
+
+                # Compute each distance
                 a_calc = np.sqrt((x_B - 0)**2 + (y_B - y_A)**2)  # A-B
                 b_calc = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2)  # B-C
                 c_calc = np.sqrt(x_C**2 + y_C**2)                 # C-D
@@ -112,30 +118,27 @@ class AnchorTagCLI:
                 e_calc = np.sqrt(x_C**2 + (y_C - y_A)**2)         # A-C
                 f_calc = np.sqrt(x_B**2 + y_B**2)                 # B-D
 
-                return [
-                    a_calc - a_measured,
-                    b_calc - b_measured,
-                    c_calc - c_measured,
-                    d_calc - d_measured,
-                    e_calc - e_measured,
-                    f_calc - f_measured
-                ]
-
-            # Run least squares optimization
-            result_noisy = least_squares(
-                error_function,
-                initial_guess,
-                args=(measured_distances_noisy,),
-                bounds=bounds,
-                loss='soft_l1'
-            )
-            optimized_coords_noisy = result_noisy.x
+                # Residuals
+                r_a = a_calc - a_measured
+                r_b = b_calc - b_measured
+                r_c = c_calc - c_measured
+                r_d = d_calc - d_measured
+                r_e = e_calc - e_measured
+                r_f = f_calc - f_measured
 
+                # Add a smoother penalty if y_B <= y_C
+                penalty = 1e3 * max(0, y_C - y_B + 10)  # Soft penalty to enforce constraint
+
+                return [r_a, r_b, r_c, r_d, r_e, r_f, penalty]
+
+            result = least_squares(error_function, initial_guess, args=(measured_distances,), bounds=(lower_bounds, upper_bounds), loss='soft_l1')
+
+            x_B, y_B, x_C, y_C, y_A = result.x
             self.anchors = {
-                "A1": (0, optimized_coords_noisy[4]),
-                "A2": (optimized_coords_noisy[0], optimized_coords_noisy[1]),
-                "A3": (optimized_coords_noisy[2], optimized_coords_noisy[3]),
-                "A4": (0, 0),
+                "A1": (0, y_A, self.anchorHeight),
+                "A2": (x_B, y_B, self.anchorHeight),
+                "A3": (x_C, y_C, self.anchorHeight),
+                "A4": (0, 0, self.anchorHeight)
             }
             return {k: (round(v[0], 2), round(v[1], 2)) for k, v in self.anchors.items()}
         except Exception as e:
@@ -155,27 +158,39 @@ class AnchorTagCLI:
         if len(available_beacons) < 3:
             return None, None
 
-        def error_function(vars):
-            x, y = vars
-            residuals = []
-            for (bx, by), d_measured in zip(available_beacons, available_distances):
-                d_computed = np.sqrt((x - bx)**2 + (y - by)**2)
-                residuals.append(d_computed - d_measured)
-            return residuals
+        heights = []
+        for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
+            horizontal_distance = np.sqrt((bx - 0)**2 + (by - 0)**2)  # Assume tag starts at (0, 0)
+            estimated_z = np.sqrt(max(0, d_measured**2 - horizontal_distance**2))  # Ensure non-negative
+            heights.append(bz - estimated_z)  # Estimate tag height relative to beacon
+
+        initial_z = max(0, min(self.anchorHeight, np.mean(heights)))  # Constrain height to [0, 200]
         
         beacon_xs = [b[0] for b in available_beacons]
         beacon_ys = [b[1] for b in available_beacons]
-        initial_guess = [np.mean(beacon_xs), np.mean(beacon_ys)]
+        initial_guess = [np.mean(beacon_xs), np.mean(beacon_ys), initial_z]
 
-        x_min = min(beacon_xs) - 100
-        x_max = max(beacon_xs) + 100
-        y_min = min(beacon_ys) - 100
-        y_max = max(beacon_ys) + 100
-        bounds = ([x_min, y_min], [x_max, y_max])
+        # Define bounds for the tag position
+        x_min = min(beacon_xs) - 1000  # Add a small margin around the beacons
+        x_max = max(beacon_xs) + 1000
+        y_min = min(beacon_ys) - 1000
+        y_max = max(beacon_ys) + 1000
+        z_min = 0.0  # Tag's height cannot be below the ground
+        z_max = self.anchorHeight  # Tag's height cannot exceed the beacon height
+        bounds = ([x_min, y_min, z_min], [x_max, y_max, z_max])
+        
+        def error_function(vars):
+            x, y, z= vars
+            residuals = []
+            for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
+                d_computed = np.sqrt((x - bx)**2 + (y - by)**2 + (z - bz)**2)
+                residuals.append(d_computed - d_measured)
+            return residuals
 
         result = least_squares(error_function, initial_guess, bounds=bounds, loss='soft_l1')
-        x_tag, y_tag = result.x
-        return x_tag, y_tag
+        # self.get_logger().info(f"Optimization result: {result.x}")
+        return tuple(result.x)
+
 
     def call_bpm(self):
         if self.testing:
@@ -216,24 +231,24 @@ class AnchorTagCLI:
                 self.tag2_distances[anchor] = distance
 
         # Now calculate both tags
-        tag1_x, tag1_y = self.calculate_tag_coordinates(self.tag_distances)
+        tag1_x, tag1_y, tag1_z = self.calculate_tag_coordinates(self.tag_distances)
         valid_tag2_distances = [dist for dist in self.tag2_distances.values() if dist > 0]
 
         # Check if there are enough valid distances for Tag 2
         if len(valid_tag2_distances) < 3:
             print(f"Insufficient valid distances for Tag 2: {len(valid_tag2_distances)} provided.")
-            tag2_x, tag2_y = None, None
+            tag2_x, tag2_y, tag2_z = None, None, None
         else:
-            tag2_x, tag2_y = self.calculate_tag_coordinates(self.tag2_distances)
+            tag2_x, tag2_y, tag2_z = self.calculate_tag_coordinates(self.tag2_distances)
 
         print("Tag Positions:")
         if tag1_x is not None and tag1_y is not None:
-            print(f"Tag 1: ({tag1_x:.2f}, {tag1_y:.2f})")
+            print(f"Tag 1: ({tag1_x:.2f}, {tag1_y:.2f}, {tag1_z:.2f})")
         else:
             print("Tag 1: Not enough data")
 
         if tag2_x is not None and tag2_y is not None:
-            print(f"Tag 2: ({tag2_x:.2f}, {tag2_y:.2f})")
+            print(f"Tag 2: ({tag2_x:.2f}, {tag2_y:.2f}, {tag2_z:.2f})")
         else:
             print("Tag 2: Not enough data")
 
@@ -241,7 +256,8 @@ class AnchorTagCLI:
         if tag1_x is not None and tag1_y is not None and tag2_x is not None and tag2_y is not None:
             dx = tag2_x - tag1_x
             dy = tag2_y - tag1_y
-            displacement = np.sqrt(dx**2 + dy**2)
+            dz = tag2_z - tag1_z
+            displacement = np.sqrt(dx**2 + dy**2 + dz**2)
             angle_deg = np.degrees(np.arctan2(dy, dx))
             if angle_deg < 0:
                 angle_deg += 360