From bfa269f6b2b6ea27c2f40c148a99982bc6e668cd Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Thu, 22 Sep 2022 11:37:20 +0200
Subject: [PATCH] Add oldHuman to be able to call propagation multiple times
between two update calls
---
source/armarx/navigation/human/HumanFilter.cpp | 8 +++++---
source/armarx/navigation/human/HumanFilter.h | 12 ++++++++++--
2 files changed, 15 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp
index 570203b6..ba786d13 100644
--- a/source/armarx/navigation/human/HumanFilter.cpp
+++ b/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&
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
index 998b491b..ceb13aa1 100644
--- a/source/armarx/navigation/human/HumanFilter.h
+++ b/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;
};
--
GitLab