Assignment1-2.py Not calculating the green arrow properly. I double-checked with the video and it looks right.

This is my output:

final_simplex: (array([[3.2474713 , 1.81209097],
[3.24747251, 1.81216234],
[3.24752673, 1.81213138]]), array([820.92784089, 820.92784096, 820.9278414 ]))
fun: 820.9278408918668
message: 'Optimization terminated successfully.'
nfev: 136
nit: 72
status: 0
success: True
x: array([3.2474713 , 1.81209097])

Here is the code:

`def distance_difference_squared(guess_location, pole, pole_measurement):

# calc distance between guess location and pole.
# compare distance vs pole_measurement
output = (guess_location[0] - pole[0])**2
output += (guess_location[1] - pole[1])**2
output += pole_measurement - np.sqrt(output)
return output**2  # square the error

def total_cost(guess_location, poles, pole_measurements):
print("guess_location: ", guess_location)
total = 0
total += distance_difference_squared(guess_location, poles[0], pole_measurements[0])
total += distance_difference_squared(guess_location, poles[1], pole_measurements[1])
total += distance_difference_squared(guess_location, poles[2], pole_measurements[2])
print("distance_difference_squared : ", total)
print("")
return total

poles = [[0, 0]]
poles += [[3, 2]]
poles += [[7, 4]]

measurements from sensors

pole_measurements = [11.180339887498949]
pole_measurements += [7.615773105863909]
pole_measurements += [3.1622776601683795]

plot_poles(poles)
plt.title('Pole Location')
plt.show()

plot_poles(poles)
plot_measurement_circles(poles, pole_measurements)
plt.title('Pole Measurements')
plt.show()

location_solution = minimize(total_cost, [0, 0], method='Nelder-Mead', args=(poles, pole_measurements))
print(location_solution)

plot_poles(poles)
plot_measurement_circles(poles, pole_measurements)
plot_robot(location_solution.x)
plt.title('Calculated Location')
plt.show()`

any help would greatly be appreciated.