diff --git a/src/publisher_gnss_tf.cpp b/src/publisher_gnss_tf.cpp
index 54da496a66ab38e699b2d01359b242d5731a7f7b..8d2a1d62e3d1fc0305641b433be38e73ed8d409d 100644
--- a/src/publisher_gnss_tf.cpp
+++ b/src/publisher_gnss_tf.cpp
@@ -66,11 +66,12 @@ void PublisherGnssTf::publishDerived()
 
     // ENU-MAP
     tf::Vector3 tf_t_enu_map;
-    tf::Matrix3x3 tf_R_enu_map;
+    tf::Quaternion tf_q_enu_map;
 
-    tf::matrixEigenToTF(sensor_gnss_->getREnuMap(),  tf_R_enu_map);
+    tf::quaternionEigenToTF(sensor_gnss_->getQEnuMap(),  tf_q_enu_map);
     tf::vectorEigenToTF(sensor_gnss_->gettEnuMap(),  tf_t_enu_map);
-    T_enu_map_ .setData(tf::Transform(tf_R_enu_map,  tf_t_enu_map));
+
+    T_enu_map_ .setData(tf::Transform(tf_q_enu_map,  tf_t_enu_map));
     T_enu_map_ .stamp_ = ros::Time::now();
     
     tfb_.sendTransform(T_enu_map_);
@@ -79,11 +80,13 @@ void PublisherGnssTf::publishDerived()
     if (publish_ecef_)
     {
         tf::Vector3 tf_t_enu_ecef;
-        tf::Matrix3x3 tf_R_enu_ecef;
-        T_ecef_enu_.setData(tf::Transform(tf_R_enu_ecef, tf_t_enu_ecef).inverse());
-        T_ecef_enu_.stamp_ = ros::Time::now();
-        tf::matrixEigenToTF(sensor_gnss_->getREnuEcef(), tf_R_enu_ecef);
+        tf::Quaternion tf_q_enu_ecef;
+
+        tf::quaternionEigenToTF(sensor_gnss_->getQEnuEcef(), tf_q_enu_ecef);
         tf::vectorEigenToTF(sensor_gnss_->gettEnuEcef(), tf_t_enu_ecef);
+        
+        T_ecef_enu_.setData(tf::Transform(tf_q_enu_ecef, tf_t_enu_ecef).inverse());
+        T_ecef_enu_.stamp_ = ros::Time::now();
     
         tfb_.sendTransform(T_ecef_enu_);
     }