diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h
index 88c893292b05b8724c126cf765ac936ea23ffd94..4d8409910eb315856fd7422f495ebbdd281b4365 100644
--- a/include/subscriber_imu.h
+++ b/include/subscriber_imu.h
@@ -36,7 +36,9 @@ namespace wolf
 class SubscriberImu : public Subscriber
 {
     protected:
-        bool flip_x_axis_, in_degrees_;
+        bool in_degrees_;
+        int x_axis_, y_axis_, z_axis_;
+        int x_axis_sgn_, y_axis_sgn_, z_axis_sgn_;
         int dim_;
         std::string cov_source_;
         bool publish_bias_;
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index e72d6775f66b641e1c2d5d06e93efc9ed9cb9a8b..2126350bd46932af13500dc2c848257be5c519fc 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -19,7 +19,6 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name,
     }
     else WOLF_ERROR("the problem's dimmension should be 2 or 3");
 
-    flip_x_axis_ = _server.getParam<bool>(prefix_ + "/flip_x_axis");
     cov_source_  = _server.getParam<std::string>(prefix_ + "/cov_source");
     assert(cov_source_ == "msg" or cov_source_=="sensor");
 
@@ -31,6 +30,84 @@ SubscriberImu::SubscriberImu(const std::string& _unique_name,
     {
         in_degrees_ = false;
     }
+
+    int imu_x_axis, imu_y_axis, imu_z_axis;
+    try
+    {
+        imu_x_axis = _server.getParam<int>(prefix_ + "/imu_x_axis");  
+        imu_y_axis = _server.getParam<int>(prefix_ + "/imu_y_axis");
+        imu_z_axis = _server.getParam<int>(prefix_ + "/imu_z_axis");  
+    }
+    catch(...)
+    {
+        imu_x_axis = 1;
+        imu_y_axis = 2;
+        imu_z_axis = 3;
+    }
+    //TODO: Assert that we have right-handed axes
+
+    if(abs(imu_x_axis) == 1) 
+    {
+      x_axis_ = 1;
+      x_axis_sgn_ = (imu_x_axis > 0 ? 1:-1);
+      if(abs(imu_y_axis) == 2){
+        assert(abs(imu_z_axis) == 3);
+        y_axis_ = 2;
+        y_axis_sgn_ = (imu_y_axis > 0 ? 1:-1);
+        z_axis_ = 3;
+        z_axis_sgn_ = (imu_z_axis > 0 ? 1:-1);
+      }
+      else{
+        assert(abs(imu_y_axis) == 3);
+        assert(abs(imu_z_axis) == 2);
+        z_axis_ = 2;
+        z_axis_sgn_ = (imu_y_axis > 0 ? 1:-1);
+        y_axis_ = 3;
+        y_axis_sgn_ = (imu_z_axis > 0 ? 1:-1);
+      }
+    }
+    else if(abs(imu_x_axis) == 2) 
+    {
+      y_axis_ = 1;
+      y_axis_sgn_ = (imu_x_axis > 0 ? 1:-1);
+      if(abs(imu_y_axis) == 1){
+        assert(abs(imu_z_axis) == 3);
+        x_axis_ = 2;
+        x_axis_sgn_ = (imu_y_axis > 0 ? 1:-1);
+        z_axis_ = 3;
+        z_axis_sgn_ = (imu_z_axis > 0 ? 1:-1);
+      }
+      else{
+        assert(abs(imu_y_axis) == 3);
+        assert(abs(imu_z_axis) == 1);
+        z_axis_ = 2;
+        z_axis_sgn_ = (imu_y_axis > 0 ? 1:-1);
+        x_axis_ = 3;
+        x_axis_sgn_ = (imu_z_axis > 0 ? 1:-1);
+      }
+    }
+    else 
+    {
+      assert(abs(imu_x_axis) == 3);
+      z_axis_ = 1;
+      z_axis_sgn_ = (imu_x_axis > 0 ? 1:-1);
+      if(abs(imu_y_axis) == 1){
+        assert(abs(imu_z_axis) == 2);
+        x_axis_ = 2;
+        x_axis_sgn_ = (imu_y_axis > 0 ? 1:-1);
+        y_axis_ = 3;
+        y_axis_sgn_ = (imu_z_axis > 0 ? 1:-1);
+      }
+      else{
+        assert(abs(imu_y_axis) == 2);
+        assert(abs(imu_z_axis) == 1);
+        y_axis_ = 2;
+        y_axis_sgn_ = (imu_y_axis > 0 ? 1:-1);
+        x_axis_ = 3;
+        x_axis_sgn_ = (imu_z_axis > 0 ? 1:-1);
+      }
+    }
+    
 }
 
 void SubscriberImu::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -47,29 +124,47 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
     ROS_DEBUG("callback Imu!");
     //Fill data
     Eigen::Vector6d data;
-      if (flip_x_axis_)
-      {
-        data << msg->linear_acceleration.x,
-             -msg->linear_acceleration.y,
-             -msg->linear_acceleration.z,
-             msg->angular_velocity.x,
-             -msg->angular_velocity.y,
-             -msg->angular_velocity.z;
-      }
-      else
-      {
-        data << msg->linear_acceleration.x,
-             msg->linear_acceleration.y,
-             msg->linear_acceleration.z,
-             msg->angular_velocity.x,
-             msg->angular_velocity.y,
-             msg->angular_velocity.z;
-      }
+    Eigen::Vector6d data_orig;
+    data_orig << msg->linear_acceleration.x,
+                 msg->linear_acceleration.y,
+                 msg->linear_acceleration.z,
+                 msg->angular_velocity.x,
+                 msg->angular_velocity.y,
+                 msg->angular_velocity.z;
+      //if (flip_x_axis_)
+      //{
+      //  data << msg->linear_acceleration.x,
+      //       -msg->linear_acceleration.y,
+      //       -msg->linear_acceleration.z,
+      //       msg->angular_velocity.x,
+      //       -msg->angular_velocity.y,
+      //       -msg->angular_velocity.z;
+      //}
+      //else
+      //{
+      //  data << msg->linear_acceleration.x,
+      //       msg->linear_acceleration.y,
+      //       msg->linear_acceleration.z,
+      //       msg->angular_velocity.x,
+      //       msg->angular_velocity.y,
+      //       msg->angular_velocity.z;
+      //}
+    data(0) = data_orig(x_axis_-1)*x_axis_sgn_;
+    data(3) = data_orig(x_axis_+2)*x_axis_sgn_;
+    data(1) = data_orig(y_axis_-1)*y_axis_sgn_;
+    data(4) = data_orig(y_axis_+2)*y_axis_sgn_;
+    data(2) = data_orig(z_axis_-1)*z_axis_sgn_;
+    data(5) = data_orig(z_axis_+2)*z_axis_sgn_;
 
-      if (in_degrees_)
-      {
-        data.tail<3>() *= M_PI/180.0;
-      }
+    WOLF_INFO("x_axis_ = ", x_axis_, " y_axis_ = ", y_axis_, " z_axis_ = ", z_axis_);
+    WOLF_INFO("data_orig = ", data_orig.transpose());
+    WOLF_INFO("data = ", data.transpose());
+
+
+    if (in_degrees_)
+    {
+      data.tail<3>() *= M_PI/180.0;
+    }
 
     //Fill cov
     Eigen::Matrix6d cov(Eigen::Matrix6d::Zero());