diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index d50bf8c20db4b131056efc6a850eff82a79a5398..82166812271da701dd5e2496fd8fcc8fc3344de9 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -270,11 +270,23 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
                     processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(proc);
                     break;
                 }
+        // abort if no processor_motion (strange...)
+        if (not processor_motion_ptr_)
+        {
+            WOLF_WARN("SubscriberImu: No processor of type ProcessorMotion in the SensorImu...?");
+            return;
+        }
+
+        // still no origin?
+        if (not processor_motion_ptr_->getOrigin())
+            return;
 
         // get current and origin states
-        auto origin_state = processor_motion_ptr_->getOrigin()->getState("PO").vector("PO");
-        auto current_state = sensor_ptr_->getProblem()->getState("PO").vector("PO");
+        auto origin_state = processor_motion_ptr_->getOrigin()->getFrame()->getState("PO");
+        auto current_state = sensor_ptr_->getProblem()->getState("PO");
         auto current_ts = sensor_ptr_->getProblem()->getTimeStamp();
+        if (not origin_state.includesStructure("PO") or not current_state.includesStructure("PO"))
+            return;
 
         // Fill message
         geometry_msgs::PoseStamped pose_msg;
@@ -283,7 +295,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
         // 3D
         if(sensor_ptr_->getProblem()->getDim() == 3)
         {
-            auto pose_from_origin = SE3::between(origin_state, current_state);
+            auto pose_from_origin = SE3::between(origin_state.vector("PO"), current_state.vector("PO"));
 
             pose_msg.pose.position.x    = pose_from_origin(0);
             pose_msg.pose.position.y    = pose_from_origin(1);
@@ -296,7 +308,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
         // 2D
         else
         {
-            auto pose_from_origin = SE2::between(origin_state, current_state);
+            auto pose_from_origin = SE2::between(origin_state.vector("PO"), current_state.vector("PO"));
 
             auto q_from_origin = Eigen::Quaterniond(Eigen::AngleAxisd(pose_from_origin(2),
                                                     Eigen::Vector3d::UnitZ()));