diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index a3992452a51aef6924c1d2a4af4541eb42894819..41f6e72823b9377182a67b30855f7b89750eb506 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -321,8 +321,13 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
         else if (std::dynamic_pointer_cast<SensorImu>(sensor_ptr_) != nullptr)
         {
-            auto q_first = Eigen::Quaterniond(Eigen::Vector4d(sensor_ptr_->getProblem()->getTrajectory()->getFirstFrame()->getStateVector("O")));
-            g_local_xy = (q_first.conjugate() * gravity()).head<2>(); 
+            g_local_xy.setZero();
+            for (auto frm_it : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
+            {
+                auto q_first = Eigen::Quaterniond(Eigen::Vector4d(frm_it.second->getStateVector("O")));
+                g_local_xy += (q_first.conjugate() * gravity()).head<2>(); 
+            }
+            g_local_xy /= sensor_ptr_->getProblem()->getTrajectory()->getFrameMap().size();
         }
         else
             return;