updated d origin

Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py

@@ -2,10 +2,10 @@ from scipy.optimize import least_squares
 import numpy as np
 
 # Measured distances arrive in order: A, E, D, B, F, C
-measured_distances = [550.00,514.78,700.00,269.26,890.22,1051.19]
+measured_distances = [1496.223841, 1503.259376, 677.756567, 945.575945, 1419.301988, 923.531267 ]
 
 # Introduce ±10 cm of noise
-noise_level = 10
+noise_level =0
 measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances))
 
 # Automatically generate a reasonable initial guess
@@ -22,33 +22,16 @@ def generate_initial_guess(measured_distances):
 
     return [x_B, y_B, x_C, y_C, y_A]
 
-# Automatically generate reasonable bounds
-def generate_bounds(measured_distances):
-    min_dist = min(measured_distances)
-    max_dist = max(measured_distances)
-
-    # Define lower and upper bounds based on measured distances
-    lower_bound = [
-        -max_dist / 2,   # x_B lower bound
-        min_dist / 2,    # y_B lower bound (above y_C)
-        -max_dist / 2,   # x_C lower bound
-        min_dist / 4,    # y_C lower bound
-        min_dist / 2     # y_A lower bound
-    ]
-    upper_bound = [
-        max_dist * 1.5,  # x_B upper bound
-        max_dist * 1.5,  # y_B upper bound
-        max_dist * 1.5,  # x_C upper bound
-        max_dist * 1.25, # y_C upper bound
-        max_dist * 1.5   # y_A upper bound
-    ]
-    return lower_bound, upper_bound
-
-# Generate the initial guess and bounds
+# Generate the initial guess
 initial_guess = generate_initial_guess(measured_distances_noisy)
-lower_bounds, upper_bounds = generate_bounds(measured_distances_noisy)
 
-# Define the error function with constraint y_B > y_C
+# Simplified uniform bounds for all variables
+bounds = ([-2000, -2000, -2000, -2000, -2000], [2000, 2000, 2000, 0, 2000])
+
+# Ensure the initial guess is within bounds
+initial_guess = np.clip(initial_guess, bounds[0], bounds[1])
+
+# Define the error function
 def error_function(variables, measured):
     x_B, y_B, x_C, y_C, y_A = variables
 
@@ -88,10 +71,11 @@ result_noisy = least_squares(
     error_function,
     initial_guess,
     args=(measured_distances_noisy,),
-    bounds=(lower_bounds, upper_bounds),
+    bounds=bounds,
     loss='soft_l1'
 )
 
+# Extract optimized coordinates
 optimized_coords_noisy = result_noisy.x
 print("Optimized coordinates with noise:", optimized_coords_noisy)