#Trajectory coordinates
TrackX = [5, 4.5, 4, 3.5, 3, 2.5, 2, 1.5, 1, 0.5, 0]
TrackY = [2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5]

#Actual position of the robot
ActualX = pos_est_robot[0]
ActualY = pos_est_robot[1]

for i in range (0, len(TrackX)):
    dx = TrackX[i]-ActualX
    dy = TrackY[i]-ActualY
    dist[i] = math.sqrt(dx**2+dy**2)

nearest_pointCoorIndex = dist.index(min(dist))
xr = TrackX[nearest_pointCoorIndex]

gamma = (2*(xr-ActualX))/(min(dist)**2)