Add oldHuman to be able to call propagation multiple times between two update calls

source/armarx/navigation/human/HumanFilter.cpp

@@ -40,18 +40,18 @@ namespace armarx::navigation::human
     HumanFilter::propagation(const armarx::core::time::DateTime& detectionTime)
     {
         double dt = (detectionTime - human.detectionTime).toSecondsDouble();
-        oldPose = human.pose;
 
         SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
         ukf->propagation(control, dt);
 
         human.pose = fromUkfState(ukf->getCurrentState());
+        human.detectionTime = detectionTime;
     }
 
     void
     HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
     {
-        double dt = (detectionTime - human.detectionTime).toSecondsDouble();
+        double dt = (detectionTime - oldHuman.detectionTime).toSecondsDouble();
 
         //update ukf with new observation
         SystemModelT::ObsT observation = toUkfObs(pose);
@@ -60,7 +60,7 @@ namespace armarx::navigation::human
         core::Pose2D newPose = fromUkfState(ukf->getCurrentState());
 
         // calculate velocity
-        Eigen::Vector2f ds = newPose.translation() - oldPose.translation();
+        Eigen::Vector2f ds = newPose.translation() - oldHuman.pose.translation();
         Eigen::Vector2f linVelocity = ds / dt;
         // apply exponential smoothing to velocity
         //TODO try other approaches?
@@ -71,6 +71,8 @@ namespace armarx::navigation::human
         human.detectionTime = detectionTime;
         human.pose = newPose;
         human.linearVelocity = newVelocity;
+
+        oldHuman = human;
     }
 
     const Human&

source/armarx/navigation/human/HumanFilter.h

@@ -64,7 +64,7 @@ namespace armarx::navigation::human
         /**
          * @brief HumanFilter::propagation propagate the system model to the given point in time. This
          * means that the human pose is updated according to the filters prediction for the given point
-         * in time
+         * in time. Should be called at most once between every two calls of HumanFilter::update.
          * @param detectionTime the point in time for which the pose should be predicted
          */
         void propagation(const DateTime& detectionTime);
@@ -92,7 +92,15 @@ namespace armarx::navigation::human
 
     private:
         Parameters params;
-        core::Pose2D oldPose;
+        /**
+         * @brief oldHuman stores information about the human at the point in time of the last
+         * HumanFilter::update call
+         */
+        Human oldHuman;
+        /**
+         * @brief human stores information about the human at the point in time of the last
+         * HumanFilter::propagation call
+         */
         Human human;
         std::unique_ptr<UnscentedKalmanFilter<SystemModelT>> ukf;
     };