Revert "Updated d_origin"

This reverts commit 3e664451.

Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py

@@ -2,54 +2,63 @@ from scipy.optimize import least_squares
 import numpy as np
 
 # Measured distances arrive in order: A, E, D, B, F, C
-measured_distances = [1496.692877, 1477.462413, 677.287532, 947.921123, 1358.796385, 922.593196]
+measured_distances = [550.00,514.78,700.00,269.26,890.22,1051.19]
 
-# Introduce ±10 cm of noise into the measurements
-noise_level = 0  # Set to zero for no noise
+# Introduce ±10 cm of noise
+noise_level = 10
 measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances))
 
-# Automatically generate a robust initial guess
+# Automatically generate a reasonable initial guess
 def generate_initial_guess(measured_distances):
     y_A = measured_distances[2]  # Distance from A to D
 
     # 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
+    y_B = measured_distances[4] / 2 + 100  # 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
+    # Guess for C based on distance C-D and A-C
+    x_C = measured_distances[5] / 2
+    y_C = measured_distances[1] / 2
 
     return [x_B, y_B, x_C, y_C, y_A]
 
-# Generate bounds based on measured distances
+# 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,       # x_B lower bound
-        -max_dist,       # y_B lower bound
-        -max_dist,       # x_C lower bound
-        -max_dist,       # y_C 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.5,  # y_C upper bound
+        max_dist * 1.25, # y_C upper bound
         max_dist * 1.5   # y_A upper bound
     ]
     return lower_bound, upper_bound
 
-# Define the error function
+# Generate the initial guess and bounds
+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
 def error_function(variables, measured):
     x_B, y_B, x_C, y_C, y_A = variables
 
-    # Map measured distances to a, e, d, b, f, c
-    a_measured, e_measured, d_measured, b_measured, f_measured, c_measured = measured
+    # Map measured distances to a,b,c,d,e,f
+    a_measured = measured[0]
+    e_measured = measured[1]
+    d_measured = measured[2]
+    b_measured = measured[3]
+    f_measured = measured[4]
+    c_measured = measured[5]
 
     # Compute each distance
     a_calc = np.sqrt((x_B - 0)**2 + (y_B - y_A)**2)  # A-B
@@ -67,19 +76,13 @@ def error_function(variables, 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
+    # Add a penalty if y_B <= y_C
+    penalty = 0
+    if y_B <= y_C:
+        penalty = 1e6 * (y_C - y_B + 1)  # Large penalty to enforce constraint
 
     return [r_a, r_b, r_c, r_d, r_e, r_f, penalty]
 
-# Generate the initial guess and bounds
-initial_guess = generate_initial_guess(measured_distances_noisy)
-lower_bounds, upper_bounds = generate_bounds(measured_distances_noisy)
-
-print("Lower bounds:", lower_bounds)
-print("Upper bounds:", upper_bounds)
-print("Initial guess:", initial_guess)
-
 # Run least squares optimization
 result_noisy = least_squares(
     error_function,
@@ -89,11 +92,8 @@ result_noisy = least_squares(
     loss='soft_l1'
 )
 
-# Extract optimized coordinates
 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)
 
-# 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)