diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 219d0b85c2c742ad10f7f8bba2a1b36f72a6ba00..0bdb0a01b9db4e03ff9afb441062c6820f8f340c 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -190,7 +190,7 @@ class Estimator {
   VectorN getQFilt() { return q_filt_dyn_; }
   VectorN getVFilt() { return v_filt_dyn_; }
   VectorN getVSecu() { return v_secu_dyn_; }
-  Vector3 getVFiltBis() { return v_filt_bis_; }
+  VectorN getVFiltBis() { return v_filt_bis_; }
   Vector3 getRPY() { return IMU_RPY_; }
   MatrixN getFeetStatus() { return feet_status_; }
   MatrixN getFeetGoals() { return feet_goals_; }
@@ -212,7 +212,7 @@ class Estimator {
   VectorN getQUpdated() { return q_up_; }
   VectorN getVRef() { return v_ref_; }
   VectorN getHV() { return h_v_; }
-  Vector3 getHVBis() { return h_v_bis_; }
+  VectorN getHVBis() { return h_v_bis_; }
   Matrix3 getoRh() { return oRh_; }
   Vector3 getoTh() { return oTh_; }
   double getYawEstim() { return yaw_estim_; }
@@ -273,8 +273,8 @@ class Estimator {
   double yaw_estim_;  // Yaw angle in perfect world
 
   int N_queue_;
-  Vector3 v_filt_bis_;
-  Vector3 h_v_bis_;
-  std::deque<double> vx_queue_, vy_queue_, vz_queue_;
+  VectorN v_filt_bis_;
+  VectorN h_v_bis_;
+  std::deque<double> vx_queue_, vy_queue_, vz_queue_, wR_queue_, wP_queue_, wY_queue_;
 };
 #endif  // ESTIMATOR_H_INCLUDED
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 1757eb65e9a2eb716e6121a9d24f1a865d8ffc71..5417f3a0f3211ff06257736b6df4fadd5e991850 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -76,8 +76,8 @@ Estimator::Estimator()
       oTh_(Vector3::Zero()),
       yaw_estim_(0.0),
       N_queue_(0),
-      v_filt_bis_(Vector3::Zero()),
-      h_v_bis_(Vector3::Zero()) {}
+      v_filt_bis_(VectorN::Zero(6, 1)),
+      h_v_bis_(VectorN::Zero(6, 1)) {}
 
 void Estimator::initialize(Params& params) {
   dt_wbc = params.dt_wbc;
@@ -88,11 +88,15 @@ void Estimator::initialize(Params& params) {
   double fc = params.fc_v_esti;  // Cut frequency
   double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
   alpha_v_ = -y + std::sqrt(y * y + 2 * y);
+  alpha_v_ = 1.0;
 
-  N_queue_ = static_cast<int>(std::round(1.0 / (dt_wbc * 3.0)));
+  N_queue_ = static_cast<int>(std::round(params.T_gait / dt_wbc));
   vx_queue_.resize(N_queue_, 0.0);  // List full of 0.0
   vy_queue_.resize(N_queue_, 0.0);  // List full of 0.0
   vz_queue_.resize(N_queue_, 0.0);  // List full of 0.0
+  wR_queue_.resize(N_queue_, 0.0);  // List full of 0.0
+  wP_queue_.resize(N_queue_, 0.0);  // List full of 0.0
+  wY_queue_.resize(N_queue_, 0.0);  // List full of 0.0
 
   // Filtering velocities used for security checks
   fc = 6.0;  // Cut frequency
@@ -356,7 +360,17 @@ void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN co
   v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
   v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
   v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
-
+  /*
+  wR_queue_.pop_back();
+  wP_queue_.pop_back();
+  wY_queue_.pop_back();
+  wR_queue_.push_front(filt_ang_vel(0));
+  wP_queue_.push_front(filt_ang_vel(1));
+  wY_queue_.push_front(filt_ang_vel(2));
+  v_filt_bis_(3) = std::accumulate(wR_queue_.begin(), wR_queue_.end(), 0.0) / N_queue_;
+  v_filt_bis_(4) = std::accumulate(wP_queue_.begin(), wP_queue_.end(), 0.0) / N_queue_;
+  v_filt_bis_(5) = std::accumulate(wY_queue_.begin(), wY_queue_.end(), 0.0) / N_queue_;*/
+  v_filt_bis_.tail(3) = filt_ang_vel;  // No filtering for angular velocity
   //////
 
   // Update model used for the forward kinematics
@@ -426,7 +440,8 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
 
     h_v_.head(3) = hRb * v_filt_dyn_.block(0, 0, 3, 1);
     h_v_.tail(3) = hRb * v_filt_dyn_.block(3, 0, 3, 1);
-    h_v_bis_ = hRb * v_filt_bis_;
+    h_v_bis_.head(3) = hRb * v_filt_bis_.block(0, 0, 3, 1);
+    h_v_bis_.tail(3) = hRb * v_filt_bis_.block(3, 0, 3, 1);
   } else {
     // TODO: Adapt static mode to new version of the code
   }