diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index d18fae814bb5defba4dea758e1a93e556bc681d5..5e0e6243898563d6562468ce43a1a66a5533e443 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -198,7 +198,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
     }
 
     // Publish IMU message without bias and gravity
-    if (pub_imu_corrected_.getNumSubscribers() != 0)
+    if (pub_imu_corrected_.getNumSubscribers() != 0 or true)
     {
         if (sensor_ptr_->getProblem()->getDim() == 3)
         {
@@ -233,7 +233,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
             imu_corrected.linear_acceleration.z = data(2);
             imu_corrected.angular_velocity.x    = data(3);
             imu_corrected.angular_velocity.y    = data(4);
-            imu_corrected.angular_velocity.z    = data(5) - bias(5);
+            imu_corrected.angular_velocity.z    = data(5) - bias(2);
 
             pub_imu_corrected_.publish(imu_corrected);
             
diff --git a/src/subscriber_imu_enableable.cpp b/src/subscriber_imu_enableable.cpp
index 73422beb7ae3148222d72eaad51868fc7465f90f..4daa5895eb0adbb3baf21e94ffac707d85bb1fdc 100644
--- a/src/subscriber_imu_enableable.cpp
+++ b/src/subscriber_imu_enableable.cpp
@@ -62,7 +62,7 @@ SubscriberImuEnableable::SubscriberImuEnableable(const std::string& _unique_name
 void SubscriberImuEnableable::initialize(ros::NodeHandle& nh, const std::string& topic)
 {
     SubscriberImu::initialize(nh,topic);
-    sub_        = nh.subscribe(topic, 1, &SubscriberImuEnableable::callback, this);
+    sub_        = nh.subscribe(topic, 10, &SubscriberImuEnableable::callback, this);
     enable_sub_ = nh.subscribe(topic_enable_, 1, &SubscriberImuEnableable::enableCallback, this);
 
     if (lowpass_filter_)