diff --git a/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py b/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py
index eda0501ddfcd1f0cb2bee8fd56f1bc3584433631..84fa45221a071d9b603659ff1d49671b3fd70696 100644
--- a/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py
+++ b/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py
@@ -2,58 +2,67 @@ 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
- b_calc = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2) # B-C
+ 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
d_calc = y_A # A-D
e_calc = np.sqrt(x_C**2 + (y_C - y_A)**2) # A-C
@@ -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)