diff --git a/aerial_robot_estimation/src/sensor/imu.cpp b/aerial_robot_estimation/src/sensor/imu.cpp
index 2e8fd9fb8..3de56673a 100644
--- a/aerial_robot_estimation/src/sensor/imu.cpp
+++ b/aerial_robot_estimation/src/sensor/imu.cpp
@@ -441,6 +441,7 @@ namespace sensor_plugin
     imu_data.header.stamp = imu_stamp_;
     tf::Quaternion q;
     base_rot_.at(0).getRotation(q);
+    q.normalize();
     tf::quaternionTFToMsg(q, imu_data.orientation);
     tf::vector3TFToMsg(omega_, imu_data.angular_velocity);
     tf::vector3TFToMsg(acc_b_, imu_data.linear_acceleration);
diff --git a/aerial_robot_estimation/src/state_estimation.cpp b/aerial_robot_estimation/src/state_estimation.cpp
index 162e20dbf..bc89a7130 100644
--- a/aerial_robot_estimation/src/state_estimation.cpp
+++ b/aerial_robot_estimation/src/state_estimation.cpp
@@ -142,6 +142,7 @@ void StateEstimator::statePublish(const ros::TimerEvent & e)
   /* Baselink */
   /* Rotation */
   tf::Quaternion q; getOrientation(Frame::BASELINK, estimate_mode_).getRotation(q);
+  q.normalize();
   tf::quaternionTFToMsg(q, odom_state.pose.pose.orientation);
   tf::vector3TFToMsg(getAngularVel(Frame::BASELINK, estimate_mode_), odom_state.twist.twist.angular);
 
@@ -171,6 +172,7 @@ void StateEstimator::statePublish(const ros::TimerEvent & e)
   /* COG */
   /* Rotation */
   getOrientation(Frame::COG, estimate_mode_).getRotation(q);
+  q.normalize();
   tf::quaternionTFToMsg(q, odom_state.pose.pose.orientation);
   tf::vector3TFToMsg(getAngularVel(Frame::COG, estimate_mode_), odom_state.twist.twist.angular);
   /* Translation */