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..1a8b432426a6105c214fff3fd680ee44db555646 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,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