updated_beacons_dorigin

Wireless_Communication/UWB/Beacons_tag_position/beacons_D_origin_modified.py

+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)