diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp
index 475abfb1af063cf104c3607968d14ea4b628436e..4ea154c014f33bfeaf6b37cec3c4ba7cf72fe58b 100644
--- a/include/qrw/Estimator.hpp
+++ b/include/qrw/Estimator.hpp
@@ -213,6 +213,7 @@ class Estimator {
   VectorN getVRef() { return v_ref_; }
   VectorN getHV() { return h_v_; }
   VectorN getHVWindowed() { return h_v_windowed_; }
+  Matrix3 getoRb() { return oRb_; }
   Matrix3 getoRh() { return oRh_; }
   Vector3 getoTh() { return oTh_; }
   double getYawEstim() { return yaw_estim_; }
@@ -232,6 +233,7 @@ class Estimator {
   Vector3 IMU_lin_acc_;                     // Linear acceleration of the IMU (gravity compensated)
   Vector3 IMU_ang_vel_;                     // Angular velocity of the IMU
   Vector3 IMU_RPY_;                         // Roll Pitch Yaw orientation of the IMU
+  Matrix3 oRb_;                             // Rotation between base and world frame
   pinocchio::SE3::Quaternion IMU_ang_pos_;  // Quaternion orientation of the IMU
   Vector12 actuators_pos_;                  // Measured positions of actuators
   Vector12 actuators_vel_;                  // Measured velocities of actuators
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index fa1128ab94f94f029d004daf3b81efad033d3c3d..8e129d5fb65aade191b9748254c03628c98a23df 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -354,6 +354,7 @@ struct EstimatorPythonVisitor : public bp::def_visitor<EstimatorPythonVisitor<Es
             .def("getVRef", &Estimator::getVRef, "")
             .def("getHV", &Estimator::getHV, "")
             .def("getHVWindowed", &Estimator::getHVWindowed, "")
+            .def("getoRb", &Estimator::getoRb, "")
             .def("getoRh", &Estimator::getoRh, "")
             .def("getoTh", &Estimator::getoTh, "")
             .def("getYawEstim", &Estimator::getYawEstim, "")
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 2adca9b783ba9a0987e774a24516e5a058646885..a809057151f29dc3333af21590aa253b7a5b357c 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -219,6 +219,7 @@ class Controller:
 
         # Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
         self.estimator.updateState(self.joystick.v_ref, self.gait)
+        oRb = self.estimator.getoRb()
         oRh = self.estimator.getoRh()
         oTh = self.estimator.getoTh().reshape((3, 1))
         self.v_ref[0:6, 0] = self.estimator.getVRef()
@@ -258,7 +259,8 @@ class Controller:
 
         t_planner = time.time()
 
-        print("iteration : " , self.k) # print iteration
+        if self.k % 250 == 0:
+            print("iteration : " , self.k) # print iteration
 
         # TODO: Add 25Hz filter for the inputs of the MPC
 
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 7453c2f4a02d029341e6f7fee285b58253cfbcee..7a52156376fbf9d18a4ddf4fa5d1fde196a1b00d 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -344,6 +344,7 @@ class LoggerControl():
             plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
             if i < 3:
                 plt.plot(t_range, self.mocap_h_v[:, i], "k", linewidth=3)
+                plt.plot(t_range, self.loop_h_v_filt_mpc[:, i], linewidth=3, color="forestgreen")
             else:
                 plt.plot(t_range, self.mocap_b_w[:, i-3], "k", linewidth=3)
 
@@ -354,7 +355,7 @@ class LoggerControl():
             # plt.plot(t_range, self.log_dq[i, :], "g", linewidth=2)
             # plt.plot(t_range[:-2], self.log_dx_invkin[i, :-2], "g", linewidth=2)
             # plt.plot(t_range[:-2], self.log_dx_ref_invkin[i, :-2], "violet", linewidth=2, linestyle="--")
-            plt.legend(["Robot state", "Robot reference state", "Ground truth", "Alternative"], prop={'size': 8})
+            plt.legend(["Robot state", "Robot reference state", "Ground truth", "Robot state (LP 15Hz)"], prop={'size': 8})
             plt.ylabel(lgd[i])
         plt.suptitle("Measured & Reference linear and angular velocities")
 
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 138eadb10edb9be77cce47556a58d22057edb14c..1a2623b5bd060d2c367d866569aac9e24c39896e 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -51,6 +51,7 @@ Estimator::Estimator()
       IMU_lin_acc_(Vector3::Zero()),
       IMU_ang_vel_(Vector3::Zero()),
       IMU_RPY_(Vector3::Zero()),
+      oRb_(Matrix3::Identity()),
       IMU_ang_pos_(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0)),
       actuators_pos_(Vector12::Zero()),
       actuators_vel_(Vector12::Zero()),
@@ -432,6 +433,9 @@ void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
     yaw_estim_ += v_ref_[5] * dt_wbc;
     q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
 
+    // Transformation matrices between world and base frames
+    oRb_ = pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), yaw_estim_);
+
     // Actuators measurements
     q_up_.tail(12) = q_filt_dyn_.tail(12);