Got GPS Math working all the way

GPS drawing math now works
Lines show speed red to green
graphics do not as bad