Add GPS velocity calc