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"\