diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 4e9b143158062cfb5d89343cb9dc8bba58a91955..fbaf83cafcb3efc3edc37c5b5199bb00a5f8f1bc 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -56,44 +56,111 @@ void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr)
     switch(processing_step_)
     {
         case FIRST_TIME_WITH_PACK :
-        {
-            KFPackPtr pack = selectPack( incoming_ptr_);
-            setOrigin(pack->key_frame);
-
-            // TODO process
-            break;
-        }
         case FIRST_TIME_WITHOUT_PACK :
-        {
-            // Create new KF for origin
-            std::cout << "PM: make KF" << std::endl;
-            VectorXs x0 = getProblem()->zeroState();
-            setOrigin(x0, _incoming_ptr->getTimeStamp());
-            break;
-        }
         case SECOND_TIME_WITH_PACK :
-        {
-            KFPackPtr pack = selectPack( last_ptr_);
-            break;
-        }
         case SECOND_TIME_WITHOUT_PACK :
         {
-            break;
+            WOLF_WARN("ProcessorMotion received data before being initialized.");
+            WOLF_INFO("Did you forget to issue a Problem::setPrior()?");
+            throw std::runtime_error("ProcessorMotion received data before being initialized.");
         }
         case RUNNING_WITH_PACK :
         {
+            ////////////////////////////////////////////////////
+            // FIRST open pack and join KF
+
             KFPackPtr pack = selectPack( last_ptr_);
+            kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
+
+            // extract pack elements TODO rename _new_keyframe
+            FrameBasePtr new_kf = pack->key_frame;
+            TimeStamp    new_ts = new_kf->getTimeStamp();
+
+            // find the capture whose buffer is affected by the new keyframe
+            auto existing_capture = findCaptureContainingTimeStamp(new_ts);
+
+            if(existing_capture == nullptr) // Keyframe without Capture --> first time
+            {
+                // TODO should be an error
+            }
+            else   // Normal operation
+            {
+                // Find the frame acting as the capture's origin
+                auto keyframe_origin = existing_capture->getOriginFramePtr();
+
+                // emplace a new motion capture to the new keyframe
+                auto new_capture = emplaceCapture(new_kf,
+                                                  getSensorPtr(),
+                                                  new_ts,
+                                                  Eigen::VectorXs::Zero(data_size_),
+                                                  existing_capture->getDataCovariance(),
+                                                  existing_capture->getCalibration(),
+                                                  existing_capture->getCalibration(),
+                                                  keyframe_origin);
+
+                // split the buffer
+                // and give the part of the buffer before the new keyframe to the key_capture
+                existing_capture->getBuffer().split(new_ts, new_capture->getBuffer());
+
+                // TODO add interpolation code ...
+
+                // create motion feature and add it to the capture
+                auto new_feature = emplaceFeature(new_capture);
+
+                // create motion constraint and add it to the feature, and constrain to the other capture (origin)
+                emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
+
+                // Update the processor pointers
+                if (existing_capture == last_ptr_)
+                    // reset processor origin
+                    origin_ptr_ = new_capture;
+
+                // Update the existing capture
+                existing_capture->setOriginFramePtr(new_kf);
+
+                // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
+                reintegrateBuffer(existing_capture);
+
+                // modify existing feature and constraint (if they exist in the existing capture)
+                if (!existing_capture->getFeatureList().empty())
+                {
+                    auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
+
+                    // Modify existing feature --------
+                    existing_feature->setMeasurement          (existing_capture->getBuffer().get().back().delta_integr_);
+                    existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
+
+                    // Modify existing constraint --------
+                    // Instead of modifying, we remove one ctr, and create a new one.
+                    auto ctr_to_remove  = existing_feature->getConstraintList().back(); // there is only one constraint!
+                    auto new_ctr        = emplaceConstraint(existing_feature, new_capture);
+                    ctr_to_remove       ->remove();  // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
+                }
+
+            }
+
+            ////////////////////////////////////////////////////
+            // NOW on with the received data
+
+            // integrate data
+            integrateOneStep();
+
+            // Update state and time stamps
+            last_ptr_->setTimeStamp(getCurrentTimeStamp());
+            last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp());
+            last_ptr_->getFramePtr()->setState(getCurrentState());
+
+            resetDerived(); // TODO see where to put this
+
             break;
         }
         case RUNNING_WITHOUT_PACK :
         {
-            preProcess();
-
             // integrate data
             integrateOneStep();
 
             // Update state and time stamps
-            last_ptr_->setTimeStamp(incoming_ptr_->getTimeStamp());
+            last_ptr_->setTimeStamp(getCurrentTimeStamp());
             last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp());
             last_ptr_->getFramePtr()->setState(getCurrentState());
 
@@ -132,20 +199,18 @@ void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr)
                 delta_integrated_cov_   . setZero();
                 jacobian_calib_         . setZero();
 
-                // reset processor origin to the new keyframe's capture
-                origin_ptr_     = last_ptr_;
-                last_ptr_       = new_capture_ptr;
-
                 // reset derived things
                 resetDerived();
 
+                // Update pointers
+                origin_ptr_     = last_ptr_;
+                last_ptr_       = new_capture_ptr;
+
                 // callback to other processors
                 getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
             }
 
 
-            postProcess();
-
             // clear incoming just in case
             incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
 
@@ -154,6 +219,9 @@ void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr)
         default :
             break;
     }
+
+    postProcess();
+
 }
 
 void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
@@ -397,6 +465,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
 bool ProcessorMotion::keyFrameCallback(FrameBasePtr _new_keyframe, const Scalar& _time_tol_other)
 {
 
+//    ProcessorBase::keyFrameCallback(_new_keyframe, _time_tol_other);
+//    return true;
+
     assert(_new_keyframe->getTrajectoryPtr() != nullptr
            && "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");