Update beacons_D_origin.py

Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py

@@ -2,10 +2,17 @@ from scipy.optimize import least_squares
 import numpy as np
 
 # Measured distances arrive in order: A, E, D, B, F, C
+<<<<<<< HEAD
 measured_distances = [1496.692877, 1477.462413, 677.287532, 947.921123, 1358.796385, 922.593196]
 
 # Introduce ±10 cm of noise into the measurements
 noise_level = 0  # Set to zero for no noise
+=======
+measured_distances = [1496.223841, 1503.259376, 677.756567, 945.575945, 1419.301988, 923.531267 ]
+
+# Introduce ±10 cm of noise
+noise_level =0
+>>>>>>> 06a45f31e79242c1c4b0f08ce03d5ddf58789e64
 measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances))
 
 # Automatically generate a robust initial guess
@@ -22,6 +29,7 @@ def generate_initial_guess(measured_distances):
 
     return [x_B, y_B, x_C, y_C, y_A]
 
+<<<<<<< HEAD
 # Generate bounds based on measured distances
 def generate_bounds(measured_distances):
     min_dist = min(measured_distances)
@@ -43,6 +51,16 @@ def generate_bounds(measured_distances):
         max_dist * 1.5   # y_A upper bound
     ]
     return lower_bound, upper_bound
+=======
+# Generate the initial guess
+initial_guess = generate_initial_guess(measured_distances_noisy)
+
+# 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])
+>>>>>>> 06a45f31e79242c1c4b0f08ce03d5ddf58789e64
 
 # Define the error function
 def error_function(variables, measured):
@@ -85,7 +103,7 @@ result_noisy = least_squares(
     error_function,
     initial_guess,
     args=(measured_distances_noisy,),
-    bounds=(lower_bounds, upper_bounds),
+    bounds=bounds,
     loss='soft_l1'
 )
 
@@ -94,6 +112,11 @@ optimized_coords_noisy = result_noisy.x
 x_B, y_B, x_C, y_C, y_A = optimized_coords_noisy
 print("Optimized coordinates with noise:", optimized_coords_noisy)
 
+<<<<<<< HEAD
 # Calculate and print residuals
 residuals_noisy = error_function(optimized_coords_noisy, measured_distances_noisy)[:-1]  # Ignore penalty
 print("\nResiduals with noisy measurements:", residuals_noisy)
+=======
+residuals_noisy = error_function(optimized_coords_noisy, measured_distances_noisy)[:-1]  # Ignore penalty in residuals
+print("Residuals with noisy measurements:", residuals_noisy)
+>>>>>>> 06a45f31e79242c1c4b0f08ce03d5ddf58789e64