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 84fa45221a071d9b603659ff1d49671b3fd70696..860c85aa133d298aba0f81ec6425fc0488127273 100644 --- a/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin.py +++ b/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 @@ -62,7 +45,7 @@ def error_function(variables, 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 + 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 @@ -88,12 +71,13 @@ 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) residuals_noisy = error_function(optimized_coords_noisy, measured_distances_noisy)[:-1] # Ignore penalty in residuals -print("Residuals with noisy measurements:", residuals_noisy) +print("Residuals with noisy measurements:", residuals_noisy) \ No newline at end of file