diff --git a/include/qrw/Joystick.hpp b/include/qrw/Joystick.hpp
index ff95e8c76b629c0331c1c988330693eef46a3f84..5ea9e3ba2ada1767ba011287c50a44453fdfd4a4 100644
--- a/include/qrw/Joystick.hpp
+++ b/include/qrw/Joystick.hpp
@@ -100,6 +100,7 @@ class Joystick {
   Vector6 p_gp_;
   Vector6 v_ref_;  // Reference velocity resulting of the polynomial interpolation
   Vector6 v_gp_;
+  Vector6 v_ref_heavy_filter_;
 
   int joystick_code_ = 0;
   bool stop_ = false;
@@ -113,6 +114,7 @@ class Joystick {
   // How much the gamepad velocity and position is filtered to avoid sharp changes
   double gp_alpha_vel = 0.0;
   double gp_alpha_pos = 0.0;
+  double gp_alpha_vel_heavy_filter = 0.001;
 
   // Maximum velocity values
   double vXScale = 0.3;  // Lateral
@@ -121,8 +123,8 @@ class Joystick {
 
   // Maximum position values
   double pRollScale = -0.32;  // Lateral
-  double pPitchScale = -0.28;  // Forward
-  double pHeightScale = 0.025;  // Forward
+  double pPitchScale = -0.32;  // Forward
+  double pHeightScale = 0.0;  // Forward
   double pYawScale = -0.35;  // Rotation
 
   // Variable to handle the automatic static/trot switching
diff --git a/src/Controller.cpp b/src/Controller.cpp
index a59c3a43a90a00df9bb2edb3140f79d1bb8c133f..23c3c2b9b8f632016ef41727d7bee89f0e22ca4d 100644
--- a/src/Controller.cpp
+++ b/src/Controller.cpp
@@ -363,4 +363,4 @@ void Controller::security_check()
     FF = Vector12::Zero();
     tau_ff = Vector12::Zero();
   }
-}
\ No newline at end of file
+}
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 96ab271ad45373cd67105becf60eb08c109d9251..83b36ed64aba6f1671b7a3b317b30a9b7fb5baaa 100644
--- a/src/Joystick.cpp
+++ b/src/Joystick.cpp
@@ -2,7 +2,8 @@
 
 Joystick::Joystick() : A3_(Vector6::Zero()), A2_(Vector6::Zero()),
                        v_ref_(Vector6::Zero()), v_gp_(Vector6::Zero()),
-                       p_ref_(Vector6::Zero()), p_gp_(Vector6::Zero()) {}
+                       p_ref_(Vector6::Zero()), p_gp_(Vector6::Zero()),
+                       v_ref_heavy_filter_(Vector6::Zero()) {}
 
 void Joystick::initialize(Params &params)
 {
@@ -164,11 +165,11 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static, Vector6 h_v)
   if (!getL1() && (k % k_mpc == 0) && (k > static_cast<int>(std::round(1.0 / params_->dt_wbc))))
   {
     // Check joysticks value to trigger the switch between static and trot
-    double v_low = 0.01;
-    double v_up = 0.03;
+    double v_low = 0.04;
+    double v_up = 0.08;
     if (!switch_static && std::abs(v_gp_(0, 0)) < v_low && std::abs(v_gp_(1, 0)) < v_low
-                       && std::abs(v_gp_(5, 0)) < v_low && std::abs(h_v(0, 0)) < v_low 
-                       && std::abs(h_v(1, 0)) < v_low && std::abs(h_v(5, 0)) < v_low)
+                       && std::abs(v_gp_(5, 0)) < v_low && std::abs(v_ref_heavy_filter_(0, 0)) < v_low 
+                       && std::abs(v_ref_heavy_filter_(1, 0)) < v_low && std::abs(v_ref_heavy_filter_(5, 0)) < v_low)
     {
       switch_static = true;
       lock_gp = true;
@@ -208,6 +209,10 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static, Vector6 h_v)
   v_ref_ = gp_alpha_vel * v_gp_ + (1 - gp_alpha_vel) * v_ref_;
   if (getL1() && gait_is_static) { v_ref_.setZero(); }
 
+  // Heavily filtered joystick velocity to be used as a trigger for the switch trot/static
+  v_ref_heavy_filter_ = gp_alpha_vel_heavy_filter * v_gp_ + (1 - gp_alpha_vel_heavy_filter) * v_ref_heavy_filter_;
+  std::cout << v_ref_heavy_filter_.transpose() << std::endl;
+
   // Low pass filter to slow down the changes of position when moving the joysticks
   p_ref_ = gp_alpha_pos * p_gp_ + (1 - gp_alpha_pos) * p_ref_;
   // std::cout << p_ref_.transpose() << std::endl;