diff --git a/src/imu_msg_fix_alg_node.cpp b/src/imu_msg_fix_alg_node.cpp
index 46f9f9b906c73a8fc4f554e848c5c26ac3f438d9..a2b79c51d777a58b4ffe963a937aa3a40703c0a1 100755
--- a/src/imu_msg_fix_alg_node.cpp
+++ b/src/imu_msg_fix_alg_node.cpp
@@ -121,9 +121,10 @@ void ImuMsgFixAlgNode::imu_in_callback(const sensor_msgs::Imu::ConstPtr& msg)
     //TODO: modify yaw, pitch or roll
     double tmp_pitch = pitch;
 
-    pitch=roll;
-    roll=tmp_pitch;
-    yaw= -yaw;
+    //pitch=-roll;
+    //roll=tmp_pitch;
+    //yaw=yaw;
+    yaw-=1.5707;
     
     tf2::Quaternion quat_tf;
     quat_tf.setRPY(roll, pitch, yaw);
@@ -132,13 +133,21 @@ void ImuMsgFixAlgNode::imu_in_callback(const sensor_msgs::Imu::ConstPtr& msg)
     this->imu_out_Imu_msg_.orientation = quat_msg;
 
 
-    this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.y;
-    this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.x;
-    this->imu_out_Imu_msg_.angular_velocity.z=-msg->angular_velocity.z;
+    //this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.y;
+    //this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.x;
+    //this->imu_out_Imu_msg_.angular_velocity.z=-msg->angular_velocity.z;
 
-    this->imu_out_Imu_msg_.linear_acceleration.x=msg->linear_acceleration.y;
-    this->imu_out_Imu_msg_.linear_acceleration.y=msg->linear_acceleration.x;
-    this->imu_out_Imu_msg_.linear_acceleration.z=-msg->linear_acceleration.z;
+    //this->imu_out_Imu_msg_.linear_acceleration.x=msg->linear_acceleration.y;
+    //this->imu_out_Imu_msg_.linear_acceleration.y=msg->linear_acceleration.x;
+    //this->imu_out_Imu_msg_.linear_acceleration.z=-msg->linear_acceleration.z;
+
+    this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.x;
+    this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.y;
+    this->imu_out_Imu_msg_.angular_velocity.z=msg->angular_velocity.z;
+
+    this->imu_out_Imu_msg_.linear_acceleration.x=msg->linear_acceleration.x;
+    this->imu_out_Imu_msg_.linear_acceleration.y=msg->linear_acceleration.y;
+    this->imu_out_Imu_msg_.linear_acceleration.z=msg->linear_acceleration.z;
   }
   else
   {