diff --git a/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin_modified.py b/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin_modified.py new file mode 100644 index 0000000000000000000000000000000000000000..eda0501ddfcd1f0cb2bee8fd56f1bc3584433631 --- /dev/null +++ b/Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin_modified.py @@ -0,0 +1,99 @@ +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] + +# Introduce ±10 cm of noise into the measurements +noise_level = 0 # Set to zero for no noise +measured_distances_noisy = measured_distances + np.random.uniform(-noise_level, noise_level, size=len(measured_distances)) + +# Automatically generate a robust 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 + + # 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 + + return [x_B, y_B, x_C, y_C, y_A] + +# Generate bounds based on measured distances +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 + 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.5 # y_A upper bound + ] + return lower_bound, upper_bound + +# Define the error function +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 + + # 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 + 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 + f_calc = np.sqrt(x_B**2 + y_B**2) # B-D + + # Residuals + r_a = a_calc - a_measured + r_b = b_calc - b_measured + r_c = c_calc - c_measured + r_d = d_calc - d_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 + + 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, + initial_guess, + args=(measured_distances_noisy,), + bounds=(lower_bounds, upper_bounds), + 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)