diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp
index 909e0c4899..2d84f490e0 100644
--- a/libraries/AC_AutoTune/AC_AutoTune.cpp
+++ b/libraries/AC_AutoTune/AC_AutoTune.cpp
@@ -1690,11 +1690,11 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float
     float sweep_time_ms = 23000;
     const float att_hold_gain = 4.5f;
     static Vector3f filt_attitude_cd;
+    static Vector2f filt_att_fdbk_from_velxy_cd;
     Vector3f attitude_cd;
     static float filt_command_reading;
     static float filt_gyro_reading;
     static float filt_tgt_rate_reading;
-    const float vel_hold_gain = 0.04f;
 
     float dwell_freq = start_frq;
     float cycle_time_ms = 0;
@@ -1707,7 +1707,7 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float
     Vector3f velocity_ned, velocity_bf;
     if (ahrs_view->get_velocity_NED(velocity_ned)) {
         velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw();
-        velocity_bf.y = velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
+        velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
     }
 
     // keep controller from requesting too high of a rate
@@ -1730,6 +1730,8 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float
         filt_attitude_cd.x += alpha * (attitude_cd.x - filt_attitude_cd.x);
         filt_attitude_cd.y += alpha * (attitude_cd.y - filt_attitude_cd.y);
         filt_attitude_cd.z += alpha * wrap_180_cd(attitude_cd.z - filt_attitude_cd.z);
+        filt_att_fdbk_from_velxy_cd.x += alpha * (-5730.0f * vel_hold_gain * velocity_bf.y - filt_att_fdbk_from_velxy_cd.x);
+        filt_att_fdbk_from_velxy_cd.y += alpha * (5730.0f * vel_hold_gain * velocity_bf.x - filt_att_fdbk_from_velxy_cd.y);
     } else {
         target_rate_cds = 0.0f;
         settle_time--;
@@ -1749,8 +1751,8 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float
             if (tune_roll_rff > 0.0f) {
                 ff_rate_contr = 5730.0f * trim_command / tune_roll_rff;
             }
-            float trim_rate_cds = ff_rate_contr + att_hold_gain * (trim_attitude_cd.x - filt_attitude_cd.x) - 5730.0f * vel_hold_gain * velocity_bf.y;
-            attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, att_hold_gain * (trim_attitude_cd.y - filt_attitude_cd.y), 0.0f);
+            float trim_rate_cds = ff_rate_contr + att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.x) - filt_attitude_cd.x);
+            attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.y) - filt_attitude_cd.y), 0.0f);
             attitude_control->rate_bf_roll_target(target_rate_cds + trim_rate_cds);
         } else {
             attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
@@ -1769,8 +1771,8 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float
             if (tune_pitch_rff > 0.0f) {
                 ff_rate_contr = 5730.0f * trim_command / tune_pitch_rff;
             }
-            float trim_rate_cds = ff_rate_contr + att_hold_gain * (trim_attitude_cd.y - filt_attitude_cd.y) + 5730.0f * vel_hold_gain * velocity_bf.x;
-            attitude_control->input_rate_bf_roll_pitch_yaw(att_hold_gain * (trim_attitude_cd.x - filt_attitude_cd.x), 0.0f, 0.0f);
+            float trim_rate_cds = ff_rate_contr + att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.y) - filt_attitude_cd.y);
+            attitude_control->input_rate_bf_roll_pitch_yaw(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.x) - filt_attitude_cd.x), 0.0f, 0.0f);
             attitude_control->rate_bf_pitch_target(target_rate_cds + trim_rate_cds);
         } else {
             attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
@@ -1790,7 +1792,7 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float
                 rp_rate_contr = 5730.0f * trim_command / tune_yaw_rp;
             }
             float trim_rate_cds = rp_rate_contr + att_hold_gain * wrap_180_cd(trim_attitude_cd.z - filt_attitude_cd.z);
-            attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
+            attitude_control->input_rate_bf_roll_pitch_yaw(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.x) - filt_attitude_cd.x), att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.y) - filt_attitude_cd.y), 0.0f);
             attitude_control->rate_bf_yaw_target(target_rate_cds + trim_rate_cds);
         } else {
             attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h
index 32f93698e9..289dc0feff 100644
--- a/libraries/AC_AutoTune/AC_AutoTune.h
+++ b/libraries/AC_AutoTune/AC_AutoTune.h
@@ -334,6 +334,7 @@ protected:
     AP_Int8  axis_bitmask;
     AP_Float aggressiveness;
     AP_Float min_d;
+    AP_Float vel_hold_gain;
 
     // copies of object pointers to make code a bit clearer
     AC_AttitudeControl *attitude_control;
diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
index cd47688351..1f134f9b65 100644
--- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
+++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
@@ -76,6 +76,13 @@ const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("MAX_GN", 4, AC_AutoTune_Heli, max_resp_gain,  1.4f),
 
+    // @Param: VELXY_P
+    // @DisplayName: AutoTune velocity xy P gain
+    // @Description: Velocity xy P gain used to hold position during Max Gain, Rate P, and Rate D frequency sweeps
+    // @Range: 0 1
+    // @User: Standard
+    AP_GROUPINFO("VELXY_P", 5, AC_AutoTune_Heli, vel_hold_gain,  0.1f),
+
     AP_GROUPEND
 };