diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp
index 42eda9ff9b..0a52cb0550 100644
--- a/AntennaTracker/Log.cpp
+++ b/AntennaTracker/Log.cpp
@@ -51,12 +51,13 @@ struct PACKED log_Vehicle_Pos {
     int32_t vehicle_lat;
     int32_t vehicle_lng;
     int32_t vehicle_alt;
-    float vehicle_heading;
-    float vehicle_speed;
+    float vehicle_vel_x;
+    float vehicle_vel_y;
+    float vehicle_vel_z;
 };
 
 // Write a vehicle pos packet
-void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, float heading, float ground_speed)
+void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel)
 {
     struct log_Vehicle_Pos pkt = {
         LOG_PACKET_HEADER_INIT(LOG_V_POS_MSG),
@@ -64,8 +65,9 @@ void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, float
         vehicle_lat     : lat,
         vehicle_lng     : lng,
         vehicle_alt     : alt,
-        vehicle_heading : heading,
-        vehicle_speed   : ground_speed,
+        vehicle_vel_x   : vel.x,
+        vehicle_vel_y   : vel.y,
+        vehicle_vel_z   : vel.z,
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -75,7 +77,7 @@ const struct LogStructure Tracker::log_structure[] = {
     {LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
         "VBAR", "Qff", "TimeUS,Press,AltDiff" },
     {LOG_V_POS_MSG, sizeof(log_Vehicle_Pos),
-        "VPOS", "QLLeff", "TimeUS,Lat,Lng,Alt,Heading,Speed" }
+        "VPOS", "QLLefff", "TimeUS,Lat,Lng,Alt,VelX,VelY,VelZ" }
 };
 
 void Tracker::Log_Write_Vehicle_Startup_Messages()
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index 30c22b070e..61947d586f 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -149,8 +149,7 @@ private:
         Location location_estimate; // lat, long in degrees * 10^7; alt in meters * 100
         uint32_t last_update_us;    // last position update in micxroseconds
         uint32_t last_update_ms;    // last position update in milliseconds
-        float heading;          // last known direction vehicle is moving
-        float ground_speed;     // vehicle's last known ground speed in m/s
+        Vector3f vel;           // the vehicle's velocity in m/s
     } vehicle;
 
     // Navigation controller state
@@ -255,7 +254,7 @@ private:
     void compass_cal_update();
     void Log_Write_Attitude();
     void Log_Write_Baro(void);
-    void Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt,float heading,float ground_speed);
+    void Log_Write_Vehicle_Pos(int32_t lat,int32_t lng,int32_t alt, const Vector3f& vel);
     void Log_Write_Vehicle_Baro(float pressure, float altitude);
     void Log_Write_Vehicle_Startup_Messages();
     void start_logging();
diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp
index 6ad4a75192..750444860f 100644
--- a/AntennaTracker/tracking.cpp
+++ b/AntennaTracker/tracking.cpp
@@ -15,7 +15,10 @@ void Tracker::update_vehicle_pos_estimate()
     if (dt < TRACKING_TIMEOUT_SEC) {
         // project the vehicle position to take account of lost radio packets
         vehicle.location_estimate = vehicle.location;
-        location_update(vehicle.location_estimate, vehicle.heading, vehicle.ground_speed * dt);
+        float east_offset = vehicle.vel.x * dt;
+        float north_offset = vehicle.vel.y * dt;
+        //location_update(vehicle.location_estimate, vehicle.heading, vehicle.ground_speed * dt);
+        location_offset(vehicle.location_estimate, north_offset, east_offset);
         // set valid_location flag
         vehicle.location_valid = true;
     } else {
@@ -124,14 +127,13 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
     vehicle.location.lat = msg.lat;
     vehicle.location.lng = msg.lon;
     vehicle.location.alt = msg.alt/10;
-    vehicle.heading      = msg.hdg * 0.01f;
-    vehicle.ground_speed = norm(msg.vx, msg.vy) * 0.01f;
+    vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
     vehicle.last_update_us = AP_HAL::micros();    
     vehicle.last_update_ms = AP_HAL::millis();
 
     // log vehicle as GPS2
     if (should_log(MASK_LOG_GPS)) {
-        Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.heading, vehicle.ground_speed);
+        Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
     }
 }