Path Graphing Test, results to be implemented in pythonDriver.py soon

pathGrapher.py

import sys
import matplotlib.pyplot as plt
from scipy.interpolate import interp1d
import matplotlib.patches as mpatches
import numpy as np
from scipy import interpolate
from scipy.signal import savgol_filter
import statsmodels.api as sm

polyDegree = 50
ptpLineColor = '#AC3010'
polyLineColor = '#3341FF'
sgLineColor = '#11F323'
lowessLineColor = '#FF0F4F'
segmentLineColor= "#F442AA"
segment2LineColor = "#3D4A70"
splineLineColor = "#333434"

plt.xlim(43.0708, 43.0744)
plt.ylim(-89.40078306000001, -89.39991273999999)
plt.title("Graphing GPS Targets")
plt.xlabel("Latitude of Point")
plt.ylabel("Longitude of Point")
patch1 = mpatches.Patch(color=ptpLineColor, label='Point to Point')
patch2 = mpatches.Patch(color=polyLineColor, label=str(polyDegree)+'th Degree Polynomial Fit')
# patch3 = mpatches.Patch(color=sgLineColor, label='Savitzky-Golay Filter')
# patch4 = mpatches.Patch(color=lowessLineColor, label='LOWESS')
patch5 = mpatches.Patch(color=segmentLineColor, label="Segmented Spline Interpolation")
patch6 = mpatches.Patch(color=segment2LineColor, label="Segmented Polynomial Fit")
patch7 = mpatches.Patch(color=splineLineColor, label="Derivative Spline Interpolation")
plt.legend(handles=[patch1, patch2, patch7, patch5, patch6])

data = np.array([(43.0709471, -89.3999523),
        (43.07096, -89.40074),
        (43.0709565, -89.4007435),
        (43.07143, -89.40072),
        (43.07155, -89.40072),
        (43.07166, -89.40071),
        (43.07177, -89.40071),
        (43.07188, -89.40069),
        (43.07209, -89.40066),
        (43.07234, -89.40061),
        (43.07244, -89.40058),
        (43.07253, -89.40057),
        (43.07259, -89.40056),
        (43.07262, -89.40056),
        (43.07269, -89.40056),
        (43.07283, -89.40056),
        (43.07286, -89.40056),
        (43.07316, -89.40055),
        (43.07327, -89.40056),
        (43.07396, -89.4006),
        (43.07403, -89.40066),
        (43.07408, -89.40064),
        (43.07415, -89.40065),
        (43.07418, -89.40065),
        (43.0741818, -89.4006487)])

#plot point to point https://stackoverflow.com/questions/18458734/python-plot-list-of-tuples/41481370
x_val = [x[0] for x in data]
y_val = [x[1] for x in data]

print (x_val)
plt.plot(x_val,y_val,polyLineColor) #nth degree polynomial fit

#plot polyline https://stackoverflow.com/questions/19165259/python-numpy-scipy-curve-fitting
x = data[:,0]
y = data[:,1]

# calculate polynomial
z = np.polyfit(x, y, polyDegree)
f = np.poly1d(z)

# calculate new x's and y's
x_new = np.linspace(x[0], x[-1], 50)
y_new = f(x_new)

plt.plot(x,y,ptpLineColor, x_new, y_new) #pTp Line

# Vars
xVals = np.array([43.0709471, 43.07096, 43.0709565, 43.07143, 43.07155, 43.07166, 43.07177, 43.07188, 43.07209, 43.07234, 43.07244, 43.07253, 43.07259, 43.07262, 43.07269, 43.07283, 43.07286, 43.07316, 43.07327, 43.07396, 43.07403, 43.07408, 43.07415, 43.07418, 43.0741818
])
x = np.sin(2*np.pi*xVals)
y = np.cos(2*np.pi*xVals)
tck, u = interpolate.splprep([x, y], s=0)
unew = np.array([-89.3999523, -89.40074, -89.4007435, -89.40072, -89.40072, -89.40071, -89.40071, -89.40069, -89.40066, -89.40061, -89.40058, -89.40057, -89.40056, -89.40056, -89.40056, -89.40056, -89.40056, -89.40055, -89.40056, -89.4006, -89.40066, -89.40064, -89.40065, -89.40065, -89.4006487])
xnew = np.arange(0, 2*np.pi, np.pi/50)
ynew = interpolate.splev(xnew, tck, der=0)

# LOWESS https://stackoverflow.com/questions/36252434/predicting-on-new-data-using-locally-weighted-regression-loess-lowess
# introduce some floats in our x-values
# x = list(range(3, 33)) + [3.2, 6.2]
# y = [1,2,1,2,1,1,3,4,5,4,5,6,5,6,7,8,9,10,11,11,12,11,11,10,12,11,11,10,9,8,2,13]

# lowess will return our "smoothed" data with a y value for at every x-value
# lowess = sm.nonparametric.lowess(y, x, frac=.3)

# unpack the lowess smoothed points to their values
# lowess_x = list(zip(*lowess))[0]
# lowess_y = list(zip(*lowess))[1]

# run scipy's interpolation. There is also extrapolation I believe
# f = interp1d(lowess_x, lowess_y, bounds_error=False)

# xnew = [i/10. for i in range(400)]

# this this generate y values for our xvalues by our interpolator
# it will MISS values outsite of the x window (less than 3, greater than 33)
# There might be a better approach, but you can run a for loop
#and if the value is out of the range, use f(min(lowess_x)) or f(max(lowess_x))
# ynew = f(xnew)


# plt.plot(x, y, 'o')
# plt.plot(lowess_x, lowess_y, '*')
# plt.plot(xnew, ynew, '-')

# Savitzky-Golay Filter https://en.wikipedia.org/wiki/Savitzky%E2%80%93Golay_filter
# yhat = savgol_filter(unew, len(t), 3) # window size 51, polynomial order 3
# yhat += 0.0004
# plt.figure(1)
# plt.plot(t, yhat, sgLineColor)

# Derivative Spline Interpolation
xVals = np.array([43.0709471, 43.07096, 43.0709565, 43.07143, 43.07155, 43.07166, 43.07177, 43.07188, 43.07209, 43.07234, 43.07244, 43.07253, 43.07259, 43.07262, 43.07269, 43.07283, 43.07286, 43.07316, 43.07327, 43.07396, 43.07403, 43.07408, 43.07415, 43.07418, 43.0741818])
unew = np.array([-89.3999523, -89.40074, -89.4007435, -89.40072, -89.40072, -89.40071, -89.40071, -89.40069, -89.40066, -89.40061, -89.40058, -89.40057, -89.40056, -89.40056, -89.40056, -89.40056, -89.40056, -89.40055, -89.40056, -89.4006, -89.40066, -89.40064, -89.40065, -89.40065, -89.4006487])

x = np.sin(2*np.pi*xVals)
y = np.cos(2*np.pi*xVals)
tck, u = interpolate.splprep([x, y], s=0)

yder = np.array([[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0], [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]])
yder.reshape(50,1)
print('yder 50,1: '+str(yder))

yder = interpolate.splev(xVals, tck, der=1)

print('yder interpolated: '+str(yder))

yder2 = np.concatenate(yder, axis=0)
yder = yder2

print('yder2: '+str(yder2))
print('yder Reshaped: '+str(yder))
print('xVals: '+str(xVals))
print('unew: '+str(unew))

yder2 = [x - 89.38242426 for x in yder]
xVals = [x / 2 + x for x in xVals]
plt.figure(1)
plt.plot(xVals[0:], yder2[0::2], splineLineColor, xVals, np.cos(xVals), splineLineColor)

# Segmented Spline Interpolation


# Segmented Polynomial Fit


plt.show()