diff --git a/libraries/AP_Beacon/AP_Beacon_SITL.cpp b/libraries/AP_Beacon/AP_Beacon_SITL.cpp
index 83dfbf2cfe..8a353b2734 100644
--- a/libraries/AP_Beacon/AP_Beacon_SITL.cpp
+++ b/libraries/AP_Beacon/AP_Beacon_SITL.cpp
@@ -100,8 +100,8 @@ void AP_Beacon_SITL::update(void)
     const Vector2f beac_diff = beacon_origin.get_distance_NE(beacon_loc);
     const Vector2f veh_diff = beacon_origin.get_distance_NE(current_loc);
 
-    Vector3f veh_pos3d(veh_diff.x, veh_diff.y, (current_loc.alt - beacon_origin.alt)*1.0e-2f);
-    Vector3f beac_pos3d(beac_diff.x, beac_diff.y, (beacon_origin.alt - beacon_loc.alt)*1.0e-2f);
+    Vector3f veh_pos3d(veh_diff.x, veh_diff.y, (beacon_origin.alt - current_loc.alt)*1.0e-2f);
+    Vector3f beac_pos3d(beac_diff.x, beac_diff.y, (beacon_loc.alt - beacon_origin.alt)*1.0e-2f);
     Vector3f beac_veh_offset = veh_pos3d - beac_pos3d;
 
     set_beacon_position(beacon_id, beac_pos3d);