diff --git a/main__1_.py b/main__1_.py deleted file mode 100644 index 51cc59fe5286a7644daa89bce72f9533723c4bdb..0000000000000000000000000000000000000000 --- a/main__1_.py +++ /dev/null @@ -1,93 +0,0 @@ -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)