def stepGradient(xNpos, yNpos, step, q_current, prnt_gr):
# q_gradient will hold derivatives of error function w.r.t.
# x0+, y0+, x1-, y1-, respectively. q_new will hold the output for
# guidepoints
q_gradient = [0] * 4
q_new = [0] * 4
n = len(xNpos)
# Now to set up the Bezier curve for variable t, notice each coefficient
# is a function of guidepoints
t = Symbol('t')
a0 = xNpos[0]
b0 = yNpos[0]
a1 = 3 * (q_current[0] - xNpos[0])
b1 = 3 * (q_current[1] - yNpos[0])
a2 = 3 * (xNpos[0] + q_current[2] - 2 * q_current[0])
b2 = 3 * (yNpos[0] + q_current[3] - 2 * q_current[1])
a3 = xNpos[n - 1] - xNpos[0] + 3 * q_current[0] - 3 * q_current[2]
b3 = yNpos[n - 1] - yNpos[0] + 3 * q_current[1] - 3 * q_current[3]
# x-coordinate function of current Bezier function
X = a0 + a1 * t + a2 * (t**2) + a3 * (t**3)
# y-coordinate function of current Bezier function
Y = b0 + b1 * t + b2 * (t**2) + b3 * (t**3)
# Create array to hold ping times (between 0 and 1) note this array starts
# at 0 and ends at (n-1)
time = makeTimeList(n)
# This loop finds the q_gradient values - the derivatives mentioned above
# (it is a loop because the values are sums)
for i in range(0, n):
q_gradient[0] += -(2.0/n) * (xNpos[i] - X.subs(t, time[i])) * \
(3*(time[i]) - 6*((time[i])**2) + 3*((time[i])**3))
q_gradient[1] += -(2.0/n) * (yNpos[i] - Y.subs(t, time[i])) * \
(3*(time[i]) - 6*((time[i])**2) + 3*((time[i])**3))
q_gradient[2] += -(2.0/n) * (xNpos[i] - X.subs(t, time[i])) * \
(3*((time[i])**2) - 3*((time[i])**3))
q_gradient[3] += -(2.0/n) * (yNpos[i] - Y.subs(t, time[i])) * \
(3*((time[i])**2) - 3*((time[i])**3))
q_new[0] = q_current[0] - (step * q_gradient[0])
q_new[1] = q_current[1] - (step * q_gradient[1])
q_new[2] = q_current[2] - (step * q_gradient[2])
q_new[3] = q_current[3] - (step * q_gradient[3])
# Print changing gradient values if requested
if (prnt_gr == 1):
print "grad x0+: " + str(q_gradient[0])
print "grad y0+: " + str(q_gradient[1])
print "grad x1-: " + str(q_gradient[2])
print "grad y1-: " + str(q_gradient[3])
# As grad_abs_sum nears zero, we approach the minimum of the error function
grad_abs_sum = math.fabs(q_gradient[0]) + math.fabs(q_gradient[1]) + \
math.fabs(q_gradient[2]) + math.fabs(q_gradient[3])
return [q_new[0], q_new[1], q_new[2], q_new[3], grad_abs_sum]
def stepGradient(xNpos, yNpos, step, q_current, prnt_gr):
# q_gradient will hold derivatives of error function w.r.t.
# x0+, y0+, x1-, y1-, respectively. q_new will hold the output for
# guidepoints
q_gradient = [0]*4
q_new = [0]*4
n = len(xNpos)
# Now to set up the Bezier curve for variable t, notice each coefficient
# is a function of guidepoints
t = Symbol('t')
a0 = xNpos[0]
b0 = yNpos[0]
a1 = 3*(q_current[0] - xNpos[0])
b1 = 3*(q_current[1] - yNpos[0])
a2 = 3*(xNpos[0] + q_current[2] - 2*q_current[0])
b2 = 3*(yNpos[0] + q_current[3] - 2*q_current[1])
a3 = xNpos[n-1] - xNpos[0] + 3*q_current[0] - 3*q_current[2]
b3 = yNpos[n-1] - yNpos[0] + 3*q_current[1] - 3*q_current[3]
# x-coordinate function of current Bezier function
X = a0 + a1*t + a2*(t**2) + a3*(t**3)
# y-coordinate function of current Bezier function
Y = b0 + b1*t + b2*(t**2) + b3*(t**3)
# Create array to hold ping times (between 0 and 1) note this array starts
# at 0 and ends at (n-1)
time = makeTimeList(n)
# This loop finds the q_gradient values - the derivatives mentioned above
# (it is a loop because the values are sums)
for i in range(0, n):
q_gradient[0] += -(2.0/n) * (xNpos[i] - X.subs(t, time[i])) * \
(3*(time[i]) - 6*((time[i])**2) + 3*((time[i])**3))
q_gradient[1] += -(2.0/n) * (yNpos[i] - Y.subs(t, time[i])) * \
(3*(time[i]) - 6*((time[i])**2) + 3*((time[i])**3))
q_gradient[2] += -(2.0/n) * (xNpos[i] - X.subs(t, time[i])) * \
(3*((time[i])**2) - 3*((time[i])**3))
q_gradient[3] += -(2.0/n) * (yNpos[i] - Y.subs(t, time[i])) * \
(3*((time[i])**2) - 3*((time[i])**3))
q_new[0] = q_current[0] - (step * q_gradient[0])
q_new[1] = q_current[1] - (step * q_gradient[1])
q_new[2] = q_current[2] - (step * q_gradient[2])
q_new[3] = q_current[3] - (step * q_gradient[3])
# Print changing gradient values if requested
if (prnt_gr == 1):
print "grad x0+: " + str(q_gradient[0])
print "grad y0+: " + str(q_gradient[1])
print "grad x1-: " + str(q_gradient[2])
print "grad y1-: " + str(q_gradient[3])
# As grad_abs_sum nears zero, we approach the minimum of the error function
grad_abs_sum = math.fabs(q_gradient[0]) + math.fabs(q_gradient[1]) + \
math.fabs(q_gradient[2]) + math.fabs(q_gradient[3])
return [q_new[0], q_new[1], q_new[2], q_new[3], grad_abs_sum]
plt.figure(1, facecolor='white')
plt.clf()
plt.plot(xpos, ypos, '-', linewidth=1.0, \
color='g', label='Actual Flight Path')
plt.plot(xNpos1, yNpos1, '.', linewidth=1.0, \
color='k', label='Noisy Data Points')
plt.xlabel('Longitude')
plt.ylabel('Latitude')
plt.legend(loc='lower right')
plt.title("Testing Conditions")
plt.show()
# PRINT SOME STUFF
time = makeTimeList(n)
print "\n\nTrue values:"
print "************"
for i in range(0, n):
print "At time[%s] (%0.2f)\tposition: (%0.3f, %0.3f)"\
%(i, time[i], xpos[i], ypos[i])
print "\n\nValues with noise:"
print "******************"
for i in range (0, n):
print "At time[%s] (%0.2f)\tposition: (%0.3f, %0.3f)"\
%(i, time[i], xNpos[i], yNpos[i])
print "\n\nCoordinates:\nTake-off longitude: %s\nTake-off latitude: %s\n"\
"Landing longitude: %s\nLanding latitude: %s\n\n"\
