diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 99c3e8e8a47eaa3dde9ae2839dd0af066a564d8f..dd2dab4972b737e2e3a64cd0bcac8c3335266634 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 99c3e8e8a47eaa3dde9ae2839dd0af066a564d8f
+Subproject commit dd2dab4972b737e2e3a64cd0bcac8c3335266634
diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol
index b96448d7c2f870db48e95be453f41e4bf689fcad..1bab3d0caade427a8836395a54973f0832138e59 100644
--- a/posix-configs/SITL/init/ekf2/standard_vtol
+++ b/posix-configs/SITL/init/ekf2/standard_vtol
@@ -19,12 +19,14 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.2
+param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.2
 param set MC_PITCH_P 6
 param set MC_ROLL_P 6
-param set MPC_XY_P 0.15
-param set MPC_XY_VEL_P 0.05
+param set MPC_XY_P 0.8
+param set MPC_ACC_HOR_MAX 2.0
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
 param set MPC_XY_VEL_D 0.005
 param set MPC_XY_FF 0.1
 param set MPC_Z_VEL_MAX 1.5
diff --git a/posix-configs/SITL/init/lpe/standard_vtol b/posix-configs/SITL/init/lpe/standard_vtol
index 0dda5325815ab952b5531d498c9e06ed1efedca0..8203ed6792004d2570d71e17cd23bdab1786f434 100644
--- a/posix-configs/SITL/init/lpe/standard_vtol
+++ b/posix-configs/SITL/init/lpe/standard_vtol
@@ -19,12 +19,14 @@ param set CAL_ACC0_YSCALE 1.01
 param set CAL_ACC0_ZSCALE 1.01
 param set CAL_ACC1_XOFF 0.01
 param set CAL_MAG0_XOFF 0.01
-param set MC_ROLLRATE_P 0.2
+param set MC_ROLLRATE_P 0.3
 param set MC_PITCHRATE_P 0.2
 param set MC_PITCH_P 6
 param set MC_ROLL_P 6
-param set MPC_XY_P 0.15
-param set MPC_XY_VEL_P 0.05
+param set MPC_XY_P 0.5
+param set MPC_ACC_HOR_MAX 1.0
+param set MPC_XY_VEL_P 0.1
+param set MPC_XY_VEL_I 0.1
 param set MPC_XY_VEL_D 0.005
 param set MPC_XY_FF 0.1
 param set MPC_Z_VEL_MAX 1.5