diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 3504e90f1f..b21f77b188 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -323,11 +323,11 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
         if (terrain_alt) {
             // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
             _origin.z -= origin_terr_offset;
-            _pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() + origin_terr_offset);
+            _pos_control.init_pos_terrain_cm(origin_terr_offset);
         } else {
             // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
             _origin.z += origin_terr_offset;
-            _pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() - origin_terr_offset);
+            _pos_control.init_pos_terrain_cm(0.0);
         }
     }
 
@@ -435,7 +435,7 @@ void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_xy()
     _destination.xy() = stopping_point;
 
     // move pos controller target horizontally
-    _pos_control.set_pos_target_xy_cm(stopping_point.x, stopping_point.y);
+    _pos_control.set_pos_desired_xy_cm(stopping_point);
 }
 
 /// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
@@ -466,10 +466,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, get_terrain_margin() * 100.0);
 
     // input shape the terrain offset
-    _pos_control.update_pos_offset_z(terr_offset);
+    _pos_control.set_pos_terrain_target_cm(terr_offset);
+
+    // get position controller's position offset (post input shaping) so it can be used in position error calculation
+    const Vector3p& psc_pos_offset = _pos_control.get_pos_offset_cm();
 
     // get current position and adjust altitude to origin and destination's frame (i.e. _frame)
-    const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset};
+    Vector3f curr_pos = _inav.get_position_neu_cm() - psc_pos_offset.tofloat();
+    curr_pos.z -= terr_offset;
     Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
     curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();
 
@@ -528,11 +532,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     target_accel *= sq(vel_scaler_dt);
     target_accel += accel_offset;
 
-    // convert final_target.z to altitude above the ekf origin
-    target_pos.z += _pos_control.get_pos_offset_z_cm();
-    target_vel.z += _pos_control.get_vel_offset_z_cms();
-    target_accel.z += _pos_control.get_accel_offset_z_cmss();
-
     // pass new target to the position controller
     _pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel);
 
@@ -778,11 +777,11 @@ bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_
         if (terrain_alt) {
             // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
             _origin.z -= origin_terr_offset;
-            _pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() + origin_terr_offset);
+            _pos_control.init_pos_terrain_cm(origin_terr_offset);
         } else {
             // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
             _origin.z += origin_terr_offset;
-            _pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() - origin_terr_offset);
+            _pos_control.init_pos_terrain_cm(0.0);
         }
     }