diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index 22d5ff5af4aa23ef23ed63b9ead4b80b81a32943..a3a32efcf77aeb157a56cfa7ae0af15c01b427ca 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -325,7 +325,12 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
         else if (std::dynamic_pointer_cast<SensorImu>(sensor_ptr_) != nullptr)
         {
-            slope = 0;
+            auto q_frame = Eigen::Quaterniond(Eigen::Vector4d(sensor_ptr_->getProblem()->getTrajectory()->getFirstFrame()->getStateVector("O")));
+            auto g_local_xy = (q_frame.conjugate() * gravity()).head<2>();
+            slope = asin(g_local_xy.norm() / gravity().norm());
+            orientation = atan2(g_local_xy(1),g_local_xy(0));
+
+            /*slope = 0;
             auto n_frames = 0;
             for (auto frm_it : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
             {
@@ -339,7 +344,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
                 
                 n_frames++;
             }
-            slope /= n_frames;
+            slope /= n_frames;*/
         }
         else
             return;