Initial implementation of GPS only velocity hold (future needs to add accel estimation)