Fixed some mathematical issues

Dependents:   GPSNavigationNew

Fork of PID by Aaron Berk