Upload New File

main__1_.py

+from scipy.optimize import least_squares
+import numpy as np
+
+# Measured distances with ±10 cm errors (a, b, c, d, e, f)
+measured_distances = [3905.12, 2236.07, 5000, 8000, 8994.27, 5590.17]
+
+# Updated initial guess for (x_B, y_B, x_C, y_C, y_D)
+# Use values that are reasonably close to the expected beacon positions.
+initial_guess = [2500, 3000, 4500, 5500, 8000]
+
+# Updated bounds for the variables
+# Allowing for a wide range, given the distances are in the thousands
+bounds = ([0, 0, 0, 0, 0], [10000, 10000, 10000, 10000, 10000])
+
+
+# Error function for least squares
+def error_function(variables, measured_distances):
+    x_B, y_B, x_C, y_C, y_D = variables
+    a_measured, b_measured, c_measured, d_measured, e_measured, f_measured = measured_distances
+
+    # Calculating residuals
+    residuals = []
+    r_a = np.sqrt(x_B**2 + y_B**2) - a_measured
+    r_b = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2) - b_measured
+    r_c = np.sqrt(x_C**2 + (y_C - y_D)**2) - c_measured
+    r_d = y_D - d_measured
+    r_e = np.sqrt(x_C**2 + y_C**2) - e_measured
+    r_f = np.sqrt(x_B**2 + (y_B - y_D)**2) - f_measured
+
+    residuals.extend([r_a, r_b, r_c, r_d, r_e, r_f])
+    return residuals
+
+
+# Run least squares optimization
+result = least_squares(
+    error_function,
+    initial_guess,
+    args=(measured_distances, ),
+    bounds=bounds,
+    loss='soft_l1'  # Using a robust loss function to deal with noise
+)
+
+# Extract optimized coordinates
+optimized_coords = result.x
+x_B, y_B, x_C, y_C, y_D = optimized_coords
+
+# Print the optimized coordinates
+print("Optimized coordinates:", optimized_coords)
+
+# Manually calculate distances based on optimized coordinates
+a_calculated = np.sqrt(x_B**2 + y_B**2)  # Distance from A to B
+b_calculated = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2)
+c_calculated = np.sqrt(x_C**2 + (y_C - y_D)**2)
+d_calculated = y_D  # Distance from A to D (since A is at (0,0) and D is at (0, y_D))
+e_calculated = np.sqrt(x_C**2 + y_C**2)
+f_calculated = np.sqrt(x_B**2 + (y_B - y_D)**2)
+
+# Verification with a tolerance to handle the measurement error (±10 cm)
+a_measured = 3905.12  # Example measured distance (in cm)
+b_measured = 2236.07
+c_measured = 5000
+d_measured = 8000  # Example measured distance (in cm)
+e_measured = 8994.27
+f_measured = 5590.17
+
+# Check if calculated distances match measured distances within ±10 cm
+assert np.isclose(
+    a_calculated, a_measured, atol=10
+), f"a_calculated {a_calculated} differs from a_measured {a_measured}"
+assert np.isclose(
+    b_calculated, b_measured, atol=10
+), f"b_calculated {b_calculated} differs from b_measured {b_measured}"
+assert np.isclose(
+    c_calculated, c_measured, atol=10
+), f"c_calculated {c_calculated} differs from c_measured {c_measured}"
+assert np.isclose(
+    d_calculated, d_measured, atol=10
+), f"d_calculated {d_calculated} differs from d_measured {d_measured}"
+assert np.isclose(
+    e_calculated, e_measured, atol=10
+), f"e_calculated {e_calculated} differs from e_measured {e_measured}"
+assert np.isclose(
+    f_calculated, f_measured, atol=10
+), f"f_calculated {f_calculated} differs from f_measured {f_measured}"
+
+print(
+    "All calculated distances are within the acceptable tolerance of the measured distances."
+)
+
+# Calculate and print the residuals after optimization
+residuals_after_optimization = error_function(optimized_coords,
+                                              measured_distances)
+print("Residuals after optimization:", residuals_after_optimization)