diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp
index 63f48902ad..669e7c646a 100644
--- a/ArduCopter/surface_tracking.cpp
+++ b/ArduCopter/surface_tracking.cpp
@@ -17,7 +17,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m.  curr_surface_alt_above_origin_cm is 7m (or 700cm)
         RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
         const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
-        const float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_position_z_up_cm() - dir * rf_state.alt_cm;
+        const float curr_surface_alt_above_origin_cm = rf_state.inertial_alt_cm - dir * rf_state.alt_cm_filt.get();
 
         // update position controller target offset to the surface's alt above the EKF origin
         copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm);