diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index e061ea23b64b4980fe421fd0af0077b6b4fe727b..52e4efdccb9d044cc6262f8953838d00e53b3043 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -9,11 +9,9 @@ armarx_add_component(dynamic_scene_provider
SOURCES
Component.cpp
ArVizDrawer.cpp
- HumanTracker.cpp
HEADERS
Component.h
ArVizDrawer.h
- HumanTracker.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 23a58b26105a5987313dd793449c41e83f2c9ceb..29c23bbf88e03ed5332dcdfb5bab22317080e7a3 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -76,18 +76,23 @@ namespace armarx::navigation::components::dynamic_scene_provider
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
// Add an optionalproperty.
- def->optional(properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
-
- def->optional(properties.laserScannerFeatures.providerName, "p.laserScannerFeatures.providerName", "");
+ def->optional(
+ properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
+
+ def->optional(properties.laserScannerFeatures.providerName,
+ "p.laserScannerFeatures.providerName",
+ "");
def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
-
+
def->optional(properties.robot.name, "p.robot.name", "");
-
+
def->optional(properties.occupancyGrid.providerName, "p.occupancyGrid.providerName", "");
def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
- def->optional(properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
- def->optional(properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
-
+ def->optional(
+ properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
+ def->optional(
+ properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
+
def->optional(properties.humanPoseProvider, "p.humanPoseProvider", "");
return def;
@@ -137,10 +142,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
*/
- while(true)
+ while (true)
{
robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
- if(robot != nullptr)
+ if (robot != nullptr)
{
break;
}
@@ -205,12 +210,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Querying humans";
- const armem::human::client::Reader::Query humanPoseQuery{.providerName = properties.humanPoseProvider,
- .timestamp = timestamp, .maxAge = Duration::MilliSeconds(500)};
+ const armem::human::client::Reader::Query humanPoseQuery{
+ .providerName = properties.humanPoseProvider,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
const armem::human::client::Reader::Result humanPoseResult =
humanPoseReaderPlugin->get().query(humanPoseQuery);
- ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success) << humanPoseResult.errorMessage;
+ ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success)
+ << humanPoseResult.errorMessage;
ARMARX_INFO << humanPoseResult.humanPoses.size() << " humans in the scene.";
@@ -265,10 +273,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Querying costmap";
- const memory::client::costmap::Reader::Query costmapQuery{.providerName =
- "distance_to_obstacle_costmap_provider", // TODO check
- .name = "distance_to_obstacles",
- .timestamp = timestamp};
+ const memory::client::costmap::Reader::Query costmapQuery{
+ .providerName = "distance_to_obstacle_costmap_provider", // TODO check
+ .name = "distance_to_obstacles",
+ .timestamp = timestamp};
const memory::client::costmap::Reader::Result costmapResult =
costmapReaderPlugin->get().query(costmapQuery);
@@ -343,11 +351,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Running human tracker";
- humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
- .humanPoses = humanPoseResult.humanPoses});
+ humanTracker.update(human::HumanTracker::Measurements{
+ .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
- humanWriterPlugin->get().store(humanTracker.getTrackedHumans(),getName(), timestamp);
+ humanWriterPlugin->get().store(humanTracker.getTrackedHumans(), getName(), timestamp);
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index bcc3ad7d5f8163380eeed9a5deb65429f6c9cd58..7d58e1bd8fc95277021458498e9d993132d8e522 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -47,7 +47,7 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
-#include "armarx/navigation/components/dynamic_scene_provider/HumanTracker.h"
+#include "armarx/navigation/human/HumanTracker.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
#include "armarx/navigation/memory/client/human/Writer.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
@@ -205,7 +205,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
- HumanTracker humanTracker;
+ human::HumanTracker humanTracker;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
deleted file mode 100644
index 197d76e06377bef4efdf5782e7bac6552a318153..0000000000000000000000000000000000000000
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ /dev/null
@@ -1,203 +0,0 @@
-#include "HumanTracker.h"
-
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-
-#include "armarx/navigation/human/types.h"
-#include <armarx/navigation/conversions/eigen.h>
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/transform.hpp>
-
-namespace armarx::navigation::components::dynamic_scene_provider
-{
-
-
- HumanTracker::DetectedHuman
- convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
- {
- const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
- ARMARX_CHECK_NOT_EMPTY(keypoints);
-
- Eigen::Vector3f centerPos;
- int size = 0;
- for (const auto& [_, v] : keypoints)
- {
- if (v.positionGlobal.has_value())
- {
- centerPos += v.positionGlobal.value().toEigen();
- size++;
- }
- }
- centerPos /= size;
-
- core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = conv::to2D(centerPos);
- //TODO: angle
- pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
-
- return {pose, humanPose.humanTrackingId, time, false};
- }
-
- void
- HumanTracker::update(const Measurements& measurements)
- {
- for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
- {
- auto& human = *it;
- if ((measurements.detectionTime - human.human.detectionTime) >=
- parameters.maxTrackingAge)
- {
- it = trackedHumans.erase(it);
- }
- else
- {
- human.associated = false;
- human.human.pose = human.human.estimateAt(measurements.detectionTime);
- it++;
- }
- }
-
- std::vector<DetectedHuman> newPoses =
- measurements.humanPoses |
- ranges::views::transform(
- [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
- { return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
- ranges::to_vector;
-
- associateHumans(newPoses);
-
- // add all not associated humans as new tracked humans
- for (const auto& detectedHuman : newPoses)
- {
- if (!detectedHuman.associated)
- {
- human::Human human{.pose = detectedHuman.pose,
- .linearVelocity = Eigen::Vector2f::Zero(),
- .detectionTime = detectedHuman.detectionTime};
-
- trackedHumans.push_back(
- {.human = human, .trackingId = detectedHuman.trackingId, .associated = true});
- }
- }
- }
-
- struct PosDistance
- {
- HumanTracker::TrackedHuman* oldHuman;
- HumanTracker::DetectedHuman* newHuman;
- float distance;
- };
-
- std::vector<PosDistance>
- getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
- std::vector<HumanTracker::DetectedHuman>& newHumans)
- {
- std::vector<PosDistance> posDistances;
-
- for (auto& oldHuman : oldHumans)
- {
- if (oldHuman.associated)
- {
- continue;
- }
- for (auto& newHuman : newHumans)
- {
- if (newHuman.associated)
- {
- continue;
- }
- posDistances.push_back(
- {&oldHuman,
- &newHuman,
- (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm()});
- }
- }
-
- std::sort(posDistances.begin(),
- posDistances.end(),
- [](const PosDistance& a, const PosDistance& b) -> bool
- { return a.distance < b.distance; });
-
- return posDistances;
- }
-
- void
- HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
- {
- // associate humans by their tracking id
- for (auto& oldHuman : trackedHumans)
- {
- if (oldHuman.associated || !oldHuman.trackingId)
- {
- continue;
- }
- for (auto& newHuman : detectedHumans)
- {
- if (newHuman.associated || !newHuman.trackingId)
- {
- continue;
- }
- if (oldHuman.trackingId.value() == newHuman.trackingId.value())
- {
- associate(&oldHuman, &newHuman);
- }
- }
- }
-
-
- // associate leftover humans by their distances
- const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
-
- for (auto& posDistance : sortedDistances)
- {
- if (posDistance.distance > parameters.maxAssociationDistance)
- {
- break;
- }
- if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
- {
- continue;
- }
- associate(posDistance.oldHuman, posDistance.newHuman);
- }
- }
-
- void
- HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
- {
- ARMARX_CHECK(!trackedHuman->associated);
- ARMARX_CHECK(!detectedHuman->associated);
-
- trackedHuman->associated = true;
- detectedHuman->associated = true;
-
- float dt =
- (detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
- Eigen::Vector2f ds =
- (detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
- Eigen::Vector2f linVelocity = ds / dt;
-
- Eigen::Vector2f velocity =
- parameters.velocityAlpha * linVelocity +
- (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
-
- trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
- trackedHuman->trackingId = detectedHuman->trackingId;
- }
-
- std::vector<human::Human>
- HumanTracker::getTrackedHumans() const
- {
- return trackedHumans |
- ranges::views::transform([](const TrackedHuman& h) -> human::Human
- { return h.human; }) |
- ranges::to_vector;
- }
-
-
- void
- HumanTracker::reset()
- {
- trackedHumans.clear();
- }
-
-} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
deleted file mode 100644
index a6991aa2f0f2cb93573a5221fc07f5929c1a0729..0000000000000000000000000000000000000000
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/**
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
- * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
- * @date 2022
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <ArmarXCore/core/time.h>
-
-#include "VisionX/libraries/armem_human/types.h"
-
-#include "armarx/navigation/core/basic_types.h"
-#include "armarx/navigation/human/types.h"
-
-namespace armarx::navigation::components::dynamic_scene_provider
-{
-
- class HumanTracker
- {
- public:
- HumanTracker() = default;
-
- struct Measurements
- {
- DateTime detectionTime;
- std::vector<armem::human::HumanPose> humanPoses;
- };
-
- struct DetectedHuman
- {
- core::Pose2D pose;
- std::optional<std::string> trackingId;
- DateTime detectionTime;
- bool associated;
- };
-
- struct TrackedHuman
- {
- human::Human human;
- std::optional<std::string> trackingId = std::nullopt;
- bool associated;
- };
-
- struct Parameters
- {
- // the duration after which tracked humans will be erased if no new measurement for this human is found
- Duration maxTrackingAge = Duration::MilliSeconds(500);
- // the maximum distance in millimeters of two human measurements to be associated with each other
- float maxAssociationDistance = 600;
- // alpha value from interval [0,1] to determine how much the new (and respectively the old) velocity should be weighted
- float velocityAlpha = 0.7;
- };
-
- void update(const Measurements& measurements);
-
- std::vector<human::Human> getTrackedHumans() const;
-
- void reset();
-
- private:
- void associateHumans(std::vector<DetectedHuman>& detectedHumans);
- void associate(TrackedHuman* tracked, DetectedHuman* detected);
-
- private:
- std::vector<TrackedHuman> trackedHumans;
- Parameters parameters;
- };
-} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index c0a1b74e0860559d872b92efa2db2f7a8e94a0d8..4cf568f3a5404f826bf970fdbdc8e7b393f1c3b1 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -11,15 +11,90 @@ armarx_add_library(teb_human
ArmarXCore
armarx_navigation::core
armarx_navigation::conversions
+ ukfm
DEPENDENCIES_PRIVATE
range-v3::range-v3
SOURCES
types.cpp
aron_conversions.cpp
ProxemicZoneCreator.cpp
+ HumanSystemModel.cpp
+ UnscentedKalmanFilter_impl.cpp
+ HumanTracker.cpp
+ HumanFilter.cpp
HEADERS
types.h
aron_conversions.h
shapes.h
ProxemicZoneCreator.h
+ HumanSystemModel.h
+ HumanTracker.h
+ HumanFilter.h
+)
+
+find_package(sciplot)
+
+armarx_add_test(so2_kalman_test
+ TEST_FILES
+ test/so2kalmanFilterTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+)
+
+armarx_add_test(se2_kalman_test
+ TEST_FILES
+ test/se2KalmanFilterTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+)
+
+armarx_add_test(se2xV_kalman_test
+ TEST_FILES
+ test/se2xVkalmanFilterTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+)
+
+armarx_add_test(manif_kalman_test
+ TEST_FILES
+ test/manifKalmanTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+)
+
+
+armarx_add_test(se3_kalman_test
+ TEST_FILES
+ test/UnscentedKalmanFilterTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ ukfm
+ sciplot::sciplot
+)
+
+
+armarx_add_test(kalman_filter_test
+ TEST_FILES
+ test/kalman_filter_test.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::teb_human
+
+ PRIVATE
+ range-v3::range-v3
)
diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba786d13976e984216d9e7ec0c0b278cc89ba67b
--- /dev/null
+++ b/source/armarx/navigation/human/HumanFilter.cpp
@@ -0,0 +1,124 @@
+#include "HumanFilter.h"
+
+namespace armarx::navigation::human
+{
+
+ HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
+ {
+ //initialize new human for detected human
+ human.pose = pose0;
+ human.linearVelocity = Eigen::Vector2f::Zero();
+ human.detectionTime = detectionTime;
+
+
+ //initialize new kalman filter for new detected human
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
+ Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= params.obs_pos_cov * params.obs_pos_cov;
+ R.block<1, 1>(2, 2) *= params.obs_rot_cov * params.obs_rot_cov;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= params.initial_state_pos_cov * params.initial_state_pos_cov;
+ P0.block<1, 1>(2, 2) *= params.initial_state_rot_cov * params.initial_state_rot_cov;
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+ UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
+
+ //initial position and orientation according to detected human
+ SystemModelT::StateT state0 = toUkfState(pose0);
+
+ ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+ }
+
+ void
+ HumanFilter::propagation(const armarx::core::time::DateTime& detectionTime)
+ {
+ double dt = (detectionTime - human.detectionTime).toSecondsDouble();
+
+ 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 - oldHuman.detectionTime).toSecondsDouble();
+
+ //update ukf with new observation
+ SystemModelT::ObsT observation = toUkfObs(pose);
+ ukf->update(observation);
+
+ core::Pose2D newPose = fromUkfState(ukf->getCurrentState());
+
+ // calculate velocity
+ Eigen::Vector2f ds = newPose.translation() - oldHuman.pose.translation();
+ Eigen::Vector2f linVelocity = ds / dt;
+ // apply exponential smoothing to velocity
+ //TODO try other approaches?
+ Eigen::Vector2f newVelocity =
+ params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
+
+ //update stored information about the human
+ human.detectionTime = detectionTime;
+ human.pose = newPose;
+ human.linearVelocity = newVelocity;
+
+ oldHuman = human;
+ }
+
+ const Human&
+ HumanFilter::get() const
+ {
+ return human;
+ }
+
+
+ HumanFilter::SystemModelT::StateT
+ HumanFilter::toUkfState(const core::Pose2D& pose)
+ {
+ // [mm] to [m]
+ return {{pose.translation().x() / 1000,
+ pose.translation().y() / 1000,
+ Eigen::Rotation2Df(pose.linear()).angle()}};
+ }
+
+ core::Pose2D
+ HumanFilter::fromUkfState(const SystemModelT::StateT& pose)
+ {
+ core::Pose2D pose2d;
+ // [m] to [mm]
+ pose2d.translation().x() = pose.pose.x() * 1000;
+ pose2d.translation().y() = pose.pose.y() * 1000;
+ pose2d.linear() = Eigen::Rotation2Df(pose.pose.angle()).toRotationMatrix();
+
+ return pose2d;
+ }
+
+ HumanFilter::SystemModelT::ControlT
+ HumanFilter::toUkfControl(const Eigen::Vector2f& vel)
+ {
+ // convert velocity from global frame to human frame (required by kalman filter)
+ const auto& global_R_human = human.pose.linear();
+ const auto& human_V_vel = global_R_human.inverse() * vel;
+
+ // [mm] to [m]
+ return {{human_V_vel.x() / 1000, human_V_vel.y() / 1000, 0}};
+ }
+
+ HumanFilter::SystemModelT::ObsT
+ HumanFilter::toUkfObs(const core::Pose2D& pose)
+ {
+ return SystemModelT::observationFunction(toUkfState(pose));
+ }
+
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..ceb13aa13478c36481812a3248fe740dafc57e8e
--- /dev/null
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -0,0 +1,108 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::human
+{
+
+ /**
+ * @brief The HumanFilter class can be used to track and filter the state of a single human. It
+ * hides implementation detail on how the filtering is done. New information about the human can
+ * be fed by using the update method. The human itself can be obtained using the get method.
+ */
+ class HumanFilter
+ {
+
+ public:
+ struct Parameters
+ {
+ //TODO which values are appropriate?
+ double control_pos_cov = 0.01;
+ double control_rot_cov = 0.01;
+ double obs_pos_cov = 0.01;
+ double obs_rot_cov = 0.01;
+ double initial_state_pos_cov = 0.01;
+ double initial_state_rot_cov = 0.01;
+ double alpha = 0.5;
+
+ float velocityAlpha = 0.7;
+ };
+
+ /**
+ * @brief HumanFilter::HumanFilter creates a new filter that filters 2D poses (position and
+ * rotation in a two dimensional plane)
+ * @param pose0 the first known pose of the human
+ * @param detectionTime the point of detection
+ */
+ HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime);
+
+ /**
+ * @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. 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);
+ /**
+ * @brief HumanFilter::update update the filter a new detected pose of the tracked human and
+ * the detection time
+ * @param pose the new detected pose of the human
+ * @param detectionTime the detection time
+ */
+ void update(const core::Pose2D& pose, const DateTime& detectionTime);
+ /**
+ * @brief HumanFilter::get returns the human whose pose is filtered containing the most recent
+ * state
+ * @return the human
+ */
+ const Human& get() const;
+
+ private:
+ using SystemModelT = kalman_filter::SystemModelSE2<double>;
+
+ SystemModelT::StateT toUkfState(const core::Pose2D& pose);
+ core::Pose2D fromUkfState(const SystemModelT::StateT& pose);
+ SystemModelT::ControlT toUkfControl(const Eigen::Vector2f& vel);
+ SystemModelT::ObsT toUkfObs(const core::Pose2D& pose);
+
+ private:
+ Parameters params;
+ /**
+ * @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;
+ };
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanSystemModel.cpp b/source/armarx/navigation/human/HumanSystemModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80f6820535832028682dad6f14dc28c0723c0cb4
--- /dev/null
+++ b/source/armarx/navigation/human/HumanSystemModel.cpp
@@ -0,0 +1,171 @@
+#include "HumanSystemModel.h"
+
+namespace armarx::navigation::human::kalman_filter
+{
+
+ // ------------------------------ SO2xR2 -----------------------------------------
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::ObsT
+ SystemModelSO2xR2<floatT>::observationFunction(const SystemModelSO2xR2::StateT& state)
+ {
+ ObsT observation;
+ observation.segment(0, 1) = state.orientation.log().coeffs();
+ observation.segment(1, 2) = state.position;
+ return observation;
+ }
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::StateT
+ SystemModelSO2xR2<floatT>::propagationFunction(const SystemModelSO2xR2::StateT& state,
+ const SystemModelSO2xR2::ControlT& control,
+ const SystemModelSO2xR2::ControlNoiseT& noise,
+ FloatT dt)
+ {
+ StateT new_state;
+ new_state.orientation = state.orientation.template rplus(control.angular_velocity * dt);
+ new_state.position = state.position + control.euclidean_velocity * dt;
+ //new_state.position =
+ // state.position + state.orientation.rotation() * control.euclidean_velocity * dt;
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::StateT
+ SystemModelSO2xR2<floatT>::retraction(const SystemModelSO2xR2::StateT& state,
+ const SystemModelSO2xR2::SigmaPointsT& sigmaPoints)
+ {
+ StateT new_state;
+ manif::SO2Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints.segment(0, 1);
+ //new_state.orientation = state.orientation.lplus(tan);
+ new_state.orientation = state.orientation.rplus(tan);
+
+ new_state.position = state.position + sigmaPoints.segment(1, 2);
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSO2xR2<floatT>::SigmaPointsT
+ SystemModelSO2xR2<floatT>::inverseRetraction(const SystemModelSO2xR2::StateT& state1,
+ const SystemModelSO2xR2::StateT& state2)
+ {
+ SigmaPointsT sigma;
+ // TODO: check if right order of substraction
+
+ //sigma.segment(0, 1) = state2.orientation.lminus(state1.orientation).coeffs();
+ sigma.segment(0, 1) = state1.orientation.lminus(state2.orientation).coeffs();
+
+ sigma.segment(1, 2) = state1.position - state2.position;
+ //sigma.segment(1, 2) = state2.position - state1.position;
+
+ return sigma;
+ }
+
+ template struct SystemModelSO2xR2<float>;
+ template struct SystemModelSO2xR2<double>;
+
+
+ // ------------------------------ SE2 -----------------------------------------
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::ObsT
+ SystemModelSE2<floatT>::observationFunction(const SystemModelSE2::StateT& state)
+ {
+ ObsT observation = state.pose.log().coeffs();
+ return observation;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::StateT
+ SystemModelSE2<floatT>::propagationFunction(const SystemModelSE2::StateT& state,
+ const SystemModelSE2::ControlT& control,
+ const SystemModelSE2::ControlNoiseT& noise,
+ FloatT dt)
+ {
+ StateT new_state;
+ new_state.pose = state.pose.template rplus(control.velocity * dt);
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::StateT
+ SystemModelSE2<floatT>::retraction(const SystemModelSE2::StateT& state,
+ const SystemModelSE2::SigmaPointsT& sigmaPoints)
+ {
+ StateT new_state;
+ manif::SE2Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints;
+ new_state.pose = state.pose.lplus(tan);
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2<floatT>::SigmaPointsT
+ SystemModelSE2<floatT>::inverseRetraction(const SystemModelSE2::StateT& state1,
+ const SystemModelSE2::StateT& state2)
+ {
+ SigmaPointsT sigma;
+ sigma = state2.pose.lminus(state1.pose).coeffs();
+ return sigma;
+ }
+
+ template struct SystemModelSE2<float>;
+ template struct SystemModelSE2<double>;
+
+
+ // ------------------------------ SE2xV -----------------------------------------
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::ObsT
+ SystemModelSE2xV<floatT>::observationFunction(const SystemModelSE2xV::StateT& state)
+ {
+ ObsT observation = state.pose.log().coeffs();
+ return observation;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::StateT
+ SystemModelSE2xV<floatT>::propagationFunction(const SystemModelSE2xV::StateT& state,
+ const SystemModelSE2xV::ControlT& control,
+ const SystemModelSE2xV::ControlNoiseT& noise,
+ FloatT dt)
+ {
+ StateT new_state;
+ new_state.pose = state.pose.template rplus(state.velocity * dt);
+ new_state.velocity = state.velocity;
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::StateT
+ SystemModelSE2xV<floatT>::retraction(const SystemModelSE2xV::StateT& state,
+ const SystemModelSE2xV::SigmaPointsT& sigmaPoints)
+ {
+ StateT new_state;
+ manif::SE2Tangent<FloatT> tan;
+ tan.coeffs() << sigmaPoints.segment(0, 3);
+ new_state.pose = state.pose.lplus(tan);
+ tan.coeffs() << sigmaPoints.segment(3, 3);
+ // TODO: this is probably not correct, i.e. there needs to be some interaction between velocity and pose
+ new_state.velocity += tan;
+ return new_state;
+ }
+
+ template <typename floatT>
+ typename SystemModelSE2xV<floatT>::SigmaPointsT
+ SystemModelSE2xV<floatT>::inverseRetraction(const SystemModelSE2xV::StateT& state1,
+ const SystemModelSE2xV::StateT& state2)
+ {
+ SigmaPointsT sigma;
+ sigma.segment(0, 3) = state2.pose.lminus(state1.pose).coeffs();
+ // TODO: this is probably not correct; probably one cannot subtract two tangent vectors at two different poses
+ // from each other
+ sigma.segment(3, 3) = (state2.velocity - state1.velocity).coeffs();
+ return sigma;
+ }
+
+ template struct SystemModelSE2xV<float>;
+ template struct SystemModelSE2xV<double>;
+
+} // namespace armarx::navigation::human::kalman_filter
diff --git a/source/armarx/navigation/human/HumanSystemModel.h b/source/armarx/navigation/human/HumanSystemModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..05de1fdcecb71f274a8b3ce26fa88cbcaee575c6
--- /dev/null
+++ b/source/armarx/navigation/human/HumanSystemModel.h
@@ -0,0 +1,171 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <manif/SE2.h>
+#include <manif/SO2.h>
+
+namespace armarx::navigation::human::kalman_filter
+{
+ //----------- SO2xR2 -----------------
+
+ template <typename floatT>
+ struct StateSO2xR2
+ {
+ manif::SO2<floatT> orientation;
+ Eigen::Matrix<floatT, 2, 1> position;
+ };
+
+ template <typename floatT>
+ struct ControlSO2xR2
+ {
+ typename manif::SO2<floatT>::Tangent angular_velocity;
+ Eigen::Matrix<floatT, 2, 1> euclidean_velocity;
+ };
+
+ template <typename floatT>
+ struct SystemModelSO2xR2
+ {
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim
+ {
+ static constexpr long state = 3, control = 3, obs = 3;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSO2xR2<FloatT>;
+ using ControlT = ControlSO2xR2<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::
+ Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+ };
+
+ extern template struct SystemModelSO2xR2<float>;
+ extern template struct SystemModelSO2xR2<double>;
+
+
+ //----------- SE2 -----------------
+
+ template <typename floatT>
+ struct StateSE2
+ {
+ manif::SE2<floatT> pose;
+ };
+
+ template <typename floatT>
+ struct ControlSE2
+ {
+ typename manif::SE2<floatT>::Tangent velocity;
+ };
+
+ template <typename floatT>
+ struct SystemModelSE2
+ {
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim
+ {
+ static constexpr long state = 3, control = 3, obs = 3;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSE2<FloatT>;
+ using ControlT = ControlSE2<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::
+ Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+ };
+
+ extern template struct SystemModelSE2<float>;
+ extern template struct SystemModelSE2<double>;
+
+
+ //----------- SE2xV -----------------
+
+ template <typename floatT>
+ struct StateSE2xV
+ {
+ manif::SE2<floatT> pose;
+ typename manif::SE2<floatT>::Tangent velocity;
+ };
+
+ template <typename floatT>
+ struct ControlSE2xV
+ {
+ };
+
+ template <typename floatT>
+ struct SystemModelSE2xV
+ {
+ static_assert(std::is_floating_point_v<floatT>);
+ struct dim
+ {
+ static constexpr long state = 6, control = 1, obs = 3;
+ };
+
+ using FloatT = floatT;
+ using StateT = StateSE2xV<FloatT>;
+ using ControlT = ControlSE2xV<FloatT>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ControlNoiseT = Eigen::
+ Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+ static ObsT observationFunction(const StateT& state);
+
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
+
+ static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+ static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+ };
+
+ extern template struct SystemModelSE2xV<float>;
+ extern template struct SystemModelSE2xV<double>;
+
+} // namespace armarx::navigation::human::kalman_filter
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2332377e9b0583eee5ae543aa4c6386723f7dd9e
--- /dev/null
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -0,0 +1,223 @@
+#include "HumanTracker.h"
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/conversions/eigen.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+namespace armarx::navigation::human
+{
+ void
+ HumanTracker::update(const Measurements& measurements)
+ {
+ // iterate over all existing tracked humans
+ for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+ {
+ auto& human = *it;
+ // when the tracked human recieved no new measurement for too long, remove it
+ if ((measurements.detectionTime - human.humanFilter.get().detectionTime) >=
+ parameters.maxTrackingAge)
+ {
+ it = trackedHumans.erase(it);
+ }
+ // otherwise the tracked human is prepared for association at the current point in time
+ else
+ {
+ human.associated = false;
+ human.humanFilter.propagation(measurements.detectionTime);
+ it++;
+ }
+ }
+
+ // calculate the poses according to the new received measurements
+ std::vector<DetectedHuman> newPoses =
+ measurements.humanPoses |
+ ranges::views::transform(
+ [measurements, this](const armem::human::HumanPose& humanPose) -> DetectedHuman {
+ return convertHumanPoseToDetectedHuman(measurements.detectionTime, humanPose);
+ }) |
+ ranges::to_vector;
+
+ // associate the new poses from the measurement with the tracked humans
+ associateHumans(newPoses);
+
+ // add all not associated poses as new tracked humans
+ for (const auto& detectedHuman : newPoses)
+ {
+ if (not detectedHuman.associated)
+ {
+ //add new tracked human to list of tracked humans
+ trackedHumans.push_back(TrackedHuman{
+ .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
+ .trackingId = detectedHuman.trackingId,
+ .associated = true});
+ }
+ }
+ }
+
+ std::vector<human::Human>
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans |
+ ranges::views::transform([](const TrackedHuman& h) -> human::Human
+ { return h.humanFilter.get(); }) |
+ ranges::to_vector;
+ }
+
+ void
+ HumanTracker::reset()
+ {
+ trackedHumans.clear();
+ }
+
+
+ HumanTracker::DetectedHuman
+ HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
+ const armem::human::HumanPose& humanPose)
+ {
+ const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+ // calculate the arithmetic mean of all keypoint positions
+ Eigen::Vector3f centerPos;
+ int size = 0;
+ for (const auto& [_, v] : keypoints)
+ {
+ if (v.positionGlobal.has_value())
+ {
+ centerPos += v.positionGlobal.value().toEigen();
+ size++;
+ }
+ }
+ centerPos /= size;
+
+ // calculate the yaw of the specified keypoint representing the orientation if it exists
+ double yaw = 0;
+ if (humanPose.keypoints.count(parameters.rotationKeypoint) > 0)
+ {
+ const armem::human::PoseKeypoint& refKeypoint =
+ humanPose.keypoints.at(parameters.rotationKeypoint);
+ ARMARX_CHECK(refKeypoint.orientationGlobal.has_value());
+ Eigen::Quaternionf qhead = refKeypoint.orientationGlobal->toEigenQuaternion();
+ Eigen::Vector3f vec(1, 0, 0);
+ Eigen::Vector3f rot3 = qhead._transformVector(vec);
+ Eigen::Vector2f rot2(rot3.x(), rot3.y());
+ if (rot2.norm() != 0)
+ {
+ // calculate angle of rot2
+ yaw = atan2(rot2.y(), rot2.x());
+ }
+ //old version using euler angles:
+ //yaw = humanPose //from all human pose keypoints
+ // .keypoints
+ // .at(parameters.rotationKeypoint) //find the keypoint representing the head
+ // .orientationGlobal //get its global orientation
+ // ->toEigen()
+ // .eulerAngles(2, 1, 0)[0]; //and extract the yaw (rotation around z axis in
+ // global coordinates)
+ }
+
+ // create the new pose with the calculated position and yaw
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = conv::to2D(centerPos);
+ pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
+
+ return {pose, humanPose.humanTrackingId, time, false};
+ }
+
+ std::vector<HumanTracker::PosDistance>
+ HumanTracker::getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans)
+ {
+ std::vector<PosDistance> posDistances;
+
+ for (auto& oldHuman : oldHumans)
+ {
+ if (oldHuman.associated)
+ {
+ continue;
+ }
+ for (auto& newHuman : newHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ // calculate distance between every possible combination of tracked and detected
+ // humans where none of them was associated
+ posDistances.push_back(
+ {&oldHuman,
+ &newHuman,
+ (newHuman.pose.translation() - oldHuman.humanFilter.get().pose.translation())
+ .norm()});
+ }
+ }
+
+ // sort the distances ascending by their numeric value
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosDistance& a, const PosDistance& b) -> bool
+ { return a.distance < b.distance; });
+
+ return posDistances;
+ }
+
+ void
+ HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
+ {
+ // first, associate humans by their tracking id
+ for (auto& oldHuman : trackedHumans)
+ {
+ if (oldHuman.associated || not oldHuman.trackingId)
+ {
+ continue;
+ }
+ for (auto& newHuman : detectedHumans)
+ {
+ if (newHuman.associated || not newHuman.trackingId)
+ {
+ continue;
+ }
+ // check for every possible combination of old and new humans (that were not already
+ // associated and have a tracking id) if their tracking id is the same
+ if (oldHuman.trackingId.value() == newHuman.trackingId.value())
+ {
+ associate(&oldHuman, &newHuman);
+ }
+ }
+ }
+
+ // second, associate leftover humans by their distances
+ const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
+
+ for (auto& posDistance : sortedDistances)
+ {
+ if (posDistance.distance > parameters.maxAssociationDistance)
+ {
+ break;
+ }
+ if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
+ {
+ continue;
+ }
+ // associate the pair with the currently smallest distance between non-associated humans
+ associate(posDistance.oldHuman, posDistance.newHuman);
+ }
+ }
+
+ void
+ HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
+ {
+ ARMARX_CHECK(not trackedHuman->associated);
+ ARMARX_CHECK(not detectedHuman->associated);
+
+ trackedHuman->associated = true;
+ detectedHuman->associated = true;
+
+ trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
+ trackedHuman->trackingId = detectedHuman->trackingId;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a83fb65b8d981a11709dd5d31efd74b29c3eb0d
--- /dev/null
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -0,0 +1,157 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::human
+{
+ using T = double;
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
+
+ /**
+ * @brief The HumanTracker class can be used to track and filter multiple humans. It hides
+ * implementation detail on how new detected humans are associated to the old, already tracked
+ * humans. New detected humans can be fed by using the update method. The tracked humans can
+ * be obtained using the getTrackedHumans method.
+ */
+ class HumanTracker
+ {
+ public:
+ HumanTracker() = default;
+
+ struct Measurements
+ {
+ DateTime detectionTime;
+ std::vector<armem::human::HumanPose> humanPoses;
+ };
+
+ struct DetectedHuman
+ {
+ core::Pose2D pose;
+ std::optional<std::string> trackingId;
+ DateTime detectionTime;
+ bool associated;
+ };
+
+ struct TrackedHuman
+ {
+ HumanFilter humanFilter;
+ std::optional<std::string> trackingId = std::nullopt;
+ bool associated;
+ };
+
+ struct PosDistance
+ {
+ HumanTracker::TrackedHuman* oldHuman;
+ HumanTracker::DetectedHuman* newHuman;
+ float distance;
+ };
+
+ struct Parameters
+ {
+ // the keypoint which should be used for calculating the rotation of the humans
+ const std::string rotationKeypoint = "HEAD";
+ // the duration after which tracked humans will be erased if no new measurement for
+ // this human is found
+ Duration maxTrackingAge = Duration::MilliSeconds(500);
+ // the maximum distance in millimeters of two human measurements where they are still
+ // associated with each other
+ float maxAssociationDistance = 600;
+ // alpha value from interval [0,1] to determine how much the current (and respectively
+ // the old) velocity should be weighted when calculating the new velocity
+ float velocityAlpha = 0.7;
+ };
+ /**
+ * @brief HumanTracker::update Updates the tracked humans with the measurements. When a
+ * measurement is close enough to an existing tracked human, they are associated, otherwise a
+ * new tracked human is created. Tracked humans that were not associated with a new measurement
+ * for a specified amount of time are removed. New associated measurements for a tracked human
+ * are always filtered to provide a less noisy state.
+ * @param measurements the new measurements of the environment
+ */
+ void update(const Measurements& measurements);
+ /**
+ * @brief HumanTracker::getTrackedHumans Returns all humans that are currently tracked.
+ * @return the tracked humans
+ */
+ std::vector<human::Human> getTrackedHumans() const;
+ /**
+ * @brief HumanTracker::reset Resets this instance to the same state as if it just would have
+ * been created.
+ */
+ void reset();
+
+ private:
+ /**
+ * @brief convertHumanPoseToDetectedHuman Sets every parameter of a detected human according
+ * to the human pose and returns the new created detected human.
+ * @param time the time of detection
+ * @param humanPose the human pose representing the human
+ * @return the new created detected human
+ */
+ DetectedHuman convertHumanPoseToDetectedHuman(const DateTime& time,
+ const armem::human::HumanPose& humanPose);
+ /**
+ * @brief getSortedDistances Returns a sorted vector of the distances between every possible
+ * combination (T, D) where T is an old, tracked human and D is a new, detected human and
+ * both of them are not already associated. The smallest distance will be the first entry in
+ * the vector
+ * @param oldHumans the old, tracked humans
+ * @param newHumans the new, detected humans
+ * @return the sorted vector of distances with references to the according humans
+ */
+ std::vector<PosDistance>
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans);
+ /**
+ * @brief HumanTracker::associateHumans Associates those tracked and detected humans that
+ * belong together.
+ * @param detectedHumans The detected humans against which the saved list of tracked humans is
+ * matched.
+ */
+ void associateHumans(std::vector<DetectedHuman>& detectedHumans);
+ /**
+ * @brief HumanTracker::associate Associates the given tracked and detected human. Therefore it
+ * updates all necessary variables of the TrackedHuman
+ * @param trackedHuman the tracked human
+ * @param detectedHuman the detected human
+ */
+ void associate(TrackedHuman* tracked, DetectedHuman* detected);
+
+
+ private:
+ std::vector<TrackedHuman> trackedHumans;
+ Parameters parameters;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5818d6edc6e852bc991b6fbcd9d752eeee1013f9
--- /dev/null
+++ b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
@@ -0,0 +1,14 @@
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp>
+
+#include <armarx/navigation/human/HumanSystemModel.h>
+
+
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSO2xR2<float>>;
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSO2xR2<double>>;
+
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSE2<double>>;
+template class UnscentedKalmanFilter<
+ armarx::navigation::human::kalman_filter::SystemModelSE2xV<double>>;
diff --git a/source/armarx/navigation/human/test/UnscentedKalmanFilterTest.cpp b/source/armarx/navigation/human/test/UnscentedKalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc1ec3847e849a6383cdee4f4f9e3bc9ef9a9542
--- /dev/null
+++ b/source/armarx/navigation/human/test/UnscentedKalmanFilterTest.cpp
@@ -0,0 +1,322 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ROBDEKON
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 18.03.21
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ *
+ * Code adapted to C++ from: https://github.com/CAOR-MINES-ParisTech/ukfm
+ * See also:
+ * - https://arxiv.org/pdf/2002.00878.pdf
+ * - https://ieeexplore.ieee.org/document/8206066/
+ */
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <sciplot/sciplot.hpp>
+
+using T = double;
+using Vector = Eigen::Matrix<T, 3, 1>;
+using Matrix = Eigen::Matrix<T, 3, 3>;
+using SystemModelT = SystemModelSE3<T>;
+
+constexpr long num_timesteps = 3000;
+constexpr T max_time = 1;
+constexpr T dt = max_time / num_timesteps;
+constexpr T c = (1 / max_time) * 2 * M_PI;
+
+constexpr T acc_noise_std = 0.01;
+constexpr T om_noise_std = 0.01;
+constexpr T obs_noise_std = 0.01;
+constexpr T initial_offset_angle = 0.0 * 10 * M_PI / 180;
+const Vector initial_offet_pos = 0.5 * Vector(1, 0.5, 0.7);
+
+void
+test_retract()
+{
+ for (int i = 0; i < num_timesteps; i++)
+ {
+ SystemModelT::StateT state1, state2;
+ state1.pose = manif::SE3<T>::Random();
+ // state1.velocity.setRandom();
+ state2.pose = manif::SE3<T>::Random();
+ // state2.velocity.setRandom();
+ // sigma = state2 - state1
+ SystemModelT::SigmaPointsT sigma = SystemModelT::inverseRetraction(state1, state2);
+ // state3 = state1 + sigma --> state3 = state2
+ SystemModelT::StateT state3 = SystemModelT::retraction(state1, sigma);
+ // ARMARX_CHECK((state2.velocity - state3.velocity).norm() < 10 * std::numeric_limits<T>::epsilon());
+ ARMARX_CHECK((state2.pose - state3.pose).coeffs().norm() <
+ 10000 * std::numeric_limits<T>::epsilon())
+ << (state2.pose - state3.pose).coeffs().norm();
+
+ SystemModelT::SigmaPointsT sigma2 = SystemModelT::inverseRetraction(state1, state3);
+ // TODO: fix bad accuracy
+ ARMARX_CHECK((sigma2 - sigma).norm() < 10000 * std::numeric_limits<T>::epsilon())
+ << (sigma2 - sigma).norm();
+ }
+}
+
+void
+test_se3()
+{
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+ const Eigen::Matrix<T, 3, 1> pos(std::sin(angle), std::cos(angle), 0);
+ manif::SO3<T> rot = manif::SO3<T>(0, 0, angle);
+ ARMARX_CHECK(std::abs(rot.log().coeffs()(2) - angle) < 1e-6);
+ manif::SE3<T> pose(pos, rot);
+
+ manif::SE3Tangent<T> tangent = pose.log();
+ ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 1e-6);
+ ARMARX_CHECK(std::abs(tangent.coeffs()(5) - angle) < 1e-6);
+ }
+}
+
+void
+simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+{
+ SystemModelT::StateT state;
+ // state.velocity = Vector(c, 0, 0);
+ state.pose = manif::SE3<T>(0, 1, 0, M_PI, M_PI, 0);
+ states.push_back(state);
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T t_prev = dt * (i - 1);
+ const T angle = t * c;
+ // yaw goes from -pi to pi
+ const T yaw = 1 * M_PI * std::sin(angle);
+ const T roll = 0.1 * M_PI * std::sin(angle) + M_PI;
+ const T pitch = 0.1 * M_PI * std::sin(angle) + M_PI;
+ const Vector pos(std::sin(angle), std::cos(angle), 0);
+ manif::SE3Tangent<T> tangent(states.at(i - 1).pose.log());
+ ARMARX_CHECK((states.at(i - 1).pose.translation() - pos).norm() < 0.1);
+ // ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 0.1);
+ const Vector vel(std::cos(angle) * c, -std::sin(angle) * c, 0);
+ const Vector acc(-std::sin(angle) * (c * c), -std::cos(angle) * (c * c), 0);
+ const Vector acc_prev(-std::sin(t_prev * c) * (c * c), -std::cos(t_prev * c) * (c * c), 0);
+ // const Eigen::Vector3f vel((pos - state.position) / dt);
+ // Eigen::Vector3f acc = (vel - state.velocity) / dt;
+ manif::SO3<T> rot = manif::SO3<T>(roll, pitch, yaw);
+ // Eigen::Matrix<T, 3, 1> rot_tan = rot.log().coeffs();
+ // ARMARX_CHECK(std::abs(rot_tan(2) - yaw) < 1e-6);
+ SystemModelT::ControlT control;
+ // control.linear_accel = state.pose.rotation().inverse() * acc_prev;
+ Vector angular_velocity(0.1 * M_PI * std::cos(angle) * c,
+ 0.1 * M_PI * std::cos(angle) * c,
+ M_PI * std::cos(angle) * c);
+ control.velocity.coeffs() << state.pose.asSO3().inverse().act(vel), angular_velocity;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+ manif::SE3Tangent<T> pro_tangent = propagated.pose.log();
+ T pos_diff1 = (propagated.pose.translation() - pos).norm();
+ // ARMARX_CHECK(pos_diff1 < pos_diff2);
+ ARMARX_CHECK(pos_diff1 < 2e-5)
+ << "Position is not the same: " << pos << " vs " << propagated.pose.translation();
+ // ARMARX_CHECK((propagated.velocity - vel).norm() < 1e-4) << "Velocity is not the same: " << vel << " vs " << propagated.velocity;
+ // ARMARX_CHECK(propagated.pose.asSO3().lminus(rot).coeffs().norm() < 1e-2) << "Rotation is not the same: " << rot.rotation() << " vs " << propagated.pose.rotation();
+ // state.velocity = vel;
+ state.pose = manif::SE3<T>(pos, rot);
+ states.push_back(propagated);
+
+ tangent = state.pose.log();
+ ARMARX_CHECK((state.pose.translation() - pos).norm() < 1e-6);
+ // ARMARX_CHECK(std::abs(tangent.coeffs()(5) - yaw) < 1e-2);
+
+ // add noise
+ control.velocity.coeffs().segment(0, 3) += acc_noise_std * Vector::Random();
+ control.velocity.coeffs().segment(3, 3) += om_noise_std * Vector::Random();
+ omegas.push_back(control);
+ }
+}
+
+void
+simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+{
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+ SystemModelT::ObsT true_obs;
+ true_obs = state.pose.log().coeffs();
+ // true_obs.segment(0, 3) = state.pose.translation();
+ // true_obs.segment(3, 3) = state.pose.asSO3().log().coeffs();
+ ARMARX_CHECK((obs - true_obs).norm() < std::numeric_limits<T>::epsilon());
+ observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
+ }
+}
+
+
+BOOST_AUTO_TEST_CASE(se3KalmanFilterTest)
+{
+ srand(time(NULL));
+
+ test_retract();
+ // test_se3();
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<3, 3>(0, 0) *= acc_noise_std * acc_noise_std;
+ Q.block<3, 3>(3, 3) *= om_noise_std * om_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std * obs_noise_std;
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<3, 3>(3, 3) *= initial_offset_angle * initial_offset_angle;
+ P0.block<3, 3>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ // P0.block<3, 3>(6, 6) *= 0.0;
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+ UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
+
+ SystemModelT::StateT state0;
+ manif::SO3<T> rot(0, 0, initial_offset_angle);
+ state0.pose = states.at(0).pose.lplus(manif::SE3<T>(initial_offet_pos, rot).log());
+ // state0.velocity = states.at(0).velocity;
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, z_true, x_obs, y_obs, z_obs, x_ukf, y_ukf, z_ukf;
+ std::vector<T> vx_true, vy_true, vz_true, vx_ukf, vy_ukf, vz_ukf;
+ std::vector<T> a_true, b_true, c_true, a_obs, b_obs, c_obs, a_ukf, b_ukf, c_ukf;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i - 1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 100 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+ const SystemModelT::StateT& current_state = ukf.getCurrentState();
+ // ARMARX_INFO << "Velocity Diff: " << (states.at(i).velocity - current_state.velocity).norm();
+ ARMARX_INFO << "Pose Diff: "
+ << (states.at(i).pose - current_state.pose).coeffs().norm();
+ // ARMARX_INFO << "Vel: " << current_state.velocity - states.at(i).velocity;
+ ARMARX_INFO << "Max Cov " << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff();
+ ARMARX_INFO
+ << "Diag: "
+ << (ukf.getCurrentStateCovariance() - Eigen::Matrix<T, 6, 6>::Identity()).norm();
+ // ARMARX_CHECK((states.at(i).position - current_state.position).norm() < 1);
+
+ x_true.push_back(states.at(i).pose.log().coeffs()(0));
+ y_true.push_back(states.at(i).pose.log().coeffs()(1));
+ z_true.push_back(states.at(i).pose.log().coeffs()(2));
+
+ x_obs.push_back(observations.at(i)(0));
+ y_obs.push_back(observations.at(i)(1));
+ z_obs.push_back(observations.at(i)(2));
+
+ x_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(0));
+ y_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(1));
+ z_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(2));
+
+ // vx_true.push_back(states.at(i).velocity(0, 0));
+ // vy_true.push_back(states.at(i).velocity(1, 0));
+ // vz_true.push_back(states.at(i).velocity(2, 0));
+ //
+ // vx_ukf.push_back(ukf.getCurrentState().velocity(0, 0));
+ // vy_ukf.push_back(ukf.getCurrentState().velocity(1, 0));
+ // vz_ukf.push_back(ukf.getCurrentState().velocity(2, 0));
+
+ a_true.push_back(states.at(i).pose.log().coeffs()(3));
+ b_true.push_back(states.at(i).pose.log().coeffs()(4));
+ c_true.push_back(states.at(i).pose.log().coeffs()(5));
+
+ a_obs.push_back(observations.at(i)(3, 0));
+ b_obs.push_back(observations.at(i)(4, 0));
+ c_obs.push_back(observations.at(i)(5, 0));
+
+ a_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(3));
+ b_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(4));
+ c_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(5));
+ TIMING_END(REST);
+ }
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot3D position_plot;
+ sciplot::Plot2D pos_plot;
+
+ position_plot.drawCurve(x_true, y_true, z_true).label("True").lineWidth(1);
+ position_plot.drawCurve(x_obs, y_obs, z_obs).label("Obs").lineWidth(1);
+ position_plot.drawCurve(x_ukf, y_ukf, z_ukf).label("UKF").lineWidth(1);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurve(x_ukf, y_ukf).label("UKF");
+
+ // sciplot::Plot3D vel_plot;
+ //
+ // vel_plot.drawCurve(vx_true, vy_true, vz_true).label("True").lineWidth(1);
+ // vel_plot.drawCurve(vx_ukf, vy_ukf, vz_ukf).label("UKF").lineWidth(1);
+
+
+ sciplot::Plot3D orientation_plot;
+
+ orientation_plot.drawCurve(a_true, b_true, c_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(a_obs, b_obs, c_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(a_ukf, b_ukf, c_ukf).label("UKF").lineWidth(1);
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot}, {position_plot}, {orientation_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(600, 600);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("kalman_output.pdf");
+}
diff --git a/source/armarx/navigation/human/test/findings/Notice.md b/source/armarx/navigation/human/test/findings/Notice.md
new file mode 100644
index 0000000000000000000000000000000000000000..61048442fd9deed08db635a2f2b1c6c172c1fc98
--- /dev/null
+++ b/source/armarx/navigation/human/test/findings/Notice.md
@@ -0,0 +1,18 @@
+# Different configurations of the kalman filter test:
+
+- start_* is the start angle of the simulated trajectory
+- mod_* is the modulo value used to define how often the kalman filter is updated
+- error is the noise added to the measurements
+ - low: 0.01
+ - high: 0.1
+- offset is the initial position offset:
+ - blank: 0.1
+ - high: 0.5 (1 for the alternative models)
+- dirty hack: skip orientations close to pi
+
+Alternative models use different calculations for retract and inverseRetract
+
+- normal: lplus(tan), 2.lminus(1), 1-2
+- alternative_model: rplus(tan), 1.lminus(2), 2-1
+- alternative_model2: rplus(tan), 1.lminus(2), 1-2
+
diff --git a/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_100-low_error-dirty_hack.pdf b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_100-low_error-dirty_hack.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..dcb8673c9ff6ed9fbb63c7d22740bb65ddfac5b2
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_100-low_error-dirty_hack.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_100-low_error.pdf b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_100-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..8727bb0cf9a7c758407d3542771e16dd6ff4ca7c
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_100-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-high_error-high_offset.pdf b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-high_error-high_offset.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..934d6ddc327f7f50022048264af1854d8fdccbdd
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-high_error-high_offset.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-high_error.pdf b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-high_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..e03efc8c2789142d0bc4ae68e80f665922019e52
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-high_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-low_error.pdf b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..dcf06f312ad2b02bf07cb0c964adc10bd30f2230
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model-start_0-mod_101-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_100-low_error-dirty_hack.pdf b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_100-low_error-dirty_hack.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..a6027ca201421ad9d2782af4304b31b09f22a614
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_100-low_error-dirty_hack.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-high_error-high_offset.pdf b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-high_error-high_offset.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..b629c7e6242145463bb012fdd78a014c93388d88
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-high_error-high_offset.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-high_error.pdf b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-high_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..643e9787e7f41d39d88fbbd0a53b6714c8021ab4
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-high_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-low_error.pdf b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..aa24b385b403d604fb3737e340d118a4ded0f878
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/alternative_model2-start_0-mod_101-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_10-low_error-dirty_hack.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_10-low_error-dirty_hack.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..cfa0646aa7a315504cbd4f24d74b949788b8922b
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_10-low_error-dirty_hack.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_10-low_error.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_10-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..3a16edf0b390d7664d6e3f452cbadbd0e8df0ae2
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_10-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_100-low_error-dirty_hack.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_100-low_error-dirty_hack.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..2b6a6085f88d032e3807c9c697596e17be2cad0d
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_100-low_error-dirty_hack.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_100-low_error.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_100-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..8da253e7f6f68998d8c5d99491f7054a938f288c
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_100-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_101-high_error-high_offset.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_101-high_error-high_offset.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..8cea1d6550d228ecc2560e0b6d161468df77155a
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_101-high_error-high_offset.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_101-high_error.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_101-high_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..5025819c3db55cb23db1b746584e3f995ca67a77
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_101-high_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_101-low_error.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_101-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..8c3dd640942f6580b83c934beb0b16b39ceba9a8
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_101-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_0-mod_11-low_error.pdf b/source/armarx/navigation/human/test/findings/start_0-mod_11-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..36aeefc8fa67abab03f8c55b611d7aee3fb55842
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_0-mod_11-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_01-mod_10-low_error.pdf b/source/armarx/navigation/human/test/findings/start_01-mod_10-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..608e403453645f5a6cd7581c3f9da443e2695c57
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_01-mod_10-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_01-mod_100-low_error.pdf b/source/armarx/navigation/human/test/findings/start_01-mod_100-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..81dc12e0c98617dbbcdc0a51400d4e2ceef72d6c
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_01-mod_100-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_pi_2-mod_10-low_error.pdf b/source/armarx/navigation/human/test/findings/start_pi_2-mod_10-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..662647f3cdbc8cee6e7eec0dbe4998665c6f9e87
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_pi_2-mod_10-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_pi_2-mod_10-low_error_dirty-hack.pdf b/source/armarx/navigation/human/test/findings/start_pi_2-mod_10-low_error_dirty-hack.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..4588d6b1975fd9b8cff13350d14858343a4ce472
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_pi_2-mod_10-low_error_dirty-hack.pdf differ
diff --git a/source/armarx/navigation/human/test/findings/start_pi_2-mod_100-low_error.pdf b/source/armarx/navigation/human/test/findings/start_pi_2-mod_100-low_error.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..c1a797f1ca66b3105a20946a6d6ea0f259472f1b
Binary files /dev/null and b/source/armarx/navigation/human/test/findings/start_pi_2-mod_100-low_error.pdf differ
diff --git a/source/armarx/navigation/human/test/kalman_filter_test.cpp b/source/armarx/navigation/human/test/kalman_filter_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04cf7c69a1d530d61d172871ec1eca58a03a8729
--- /dev/null
+++ b/source/armarx/navigation/human/test/kalman_filter_test.cpp
@@ -0,0 +1,112 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
+#include <armarx/navigation/human/types.h>
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+using armarx::navigation::core::Pose2D;
+using Eigen::Vector2f;
+
+namespace armarx::navigation::human
+{
+ Pose2D
+ generatePose(double x, double y, double yaw)
+ {
+ Pose2D pose = Pose2D::Identity();
+ pose.translation() = Eigen::Vector2f(x, y);
+ pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
+
+ return pose;
+ }
+
+ DateTime
+ generateTime(double time)
+ {
+ return DateTime(time);
+ }
+
+ double
+ extractRotation(const Pose2D& pose)
+ {
+ return Eigen::Rotation2Df(pose.linear()).angle();
+ }
+
+
+ bool
+ doublesEqual(const double d1, const double d2, const double relErr)
+ {
+ if (d1 == 0)
+ {
+ return d2 == 0;
+ }
+ return std::abs(d1 - d2) / d1 <= relErr;
+ }
+
+ bool
+ posesEqual(const Pose2D& p1, const Pose2D& p2, const double relErr)
+ {
+ return doublesEqual(p1.translation().x(), p2.translation().x(), relErr) &&
+ doublesEqual(p1.translation().y(), p2.translation().y(), relErr) &&
+ doublesEqual(extractRotation(p1), extractRotation(p2), relErr);
+ }
+
+ bool
+ vectorsEqual(const Vector2f& v1, const Vector2f v2, const double relErr)
+ {
+ return doublesEqual(v1.x(), v2.x(), relErr) && doublesEqual(v1.y(), v2.y(), relErr);
+ }
+
+ bool
+ timesEqual(const DateTime& t1, const DateTime& t2, const double relErr)
+ {
+ return doublesEqual(t1.toMilliSecondsSinceEpoch(), t2.toMilliSecondsSinceEpoch(), relErr);
+ }
+
+ bool
+ humansEqual(const Human& h1, const Human& h2, const double relErr)
+ {
+ return posesEqual(h1.pose, h2.pose, relErr) &&
+ vectorsEqual(h1.linearVelocity, h2.linearVelocity, relErr) &&
+ timesEqual(h1.detectionTime, h2.detectionTime, relErr);
+ }
+
+
+ BOOST_AUTO_TEST_CASE(testInitialization)
+ {
+ HumanFilter filter(generatePose(3, 2, 1), generateTime(4));
+
+ Human human{.pose = generatePose(3, 2, 1),
+ .linearVelocity = Vector2f::Zero(),
+ .detectionTime = generateTime(4)};
+
+ BOOST_TEST(humansEqual(filter.get(), human, 0.001),
+ "filter should not have changed humans initial pose properties");
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/manifKalmanTest.cpp b/source/armarx/navigation/human/test/manifKalmanTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4940ab7cd36227965209ac837a676b265816f06a
--- /dev/null
+++ b/source/armarx/navigation/human/test/manifKalmanTest.cpp
@@ -0,0 +1,462 @@
+/**
+ * \file se2_localization_ukfm.cpp
+ *
+ * Created on: Dec 10, 2018
+ * \author: artivis
+ *
+ * ---------------------------------------------------------
+ * This file is:
+ * (c) 2021 Jeremie Deray
+ *
+ * This file is part of `manif`, a C++ template-only library
+ * for Lie theory targeted at estimation for robotics.
+ * Manif is:
+ * (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
+ * ---------------------------------------------------------
+ *
+ * ---------------------------------------------------------
+ * Demonstration example:
+ *
+ * 2D Robot localization based on fixed beacons.
+ *
+ * See se3_localization.cpp for the 3D equivalent.
+ * See se3_sam.cpp for a more advanced example performing smoothing and mapping.
+ * ---------------------------------------------------------
+ *
+ * This demo showcases an application of an Unscented Kalman Filter on Manifold,
+ * based on the paper
+ * 'A Code for Unscented Kalman Filtering on Manifolds (UKF-M)'
+ * [https://arxiv.org/pdf/2002.00878.pdf], M. Brossard, A. Barrau and S. Bonnabel.
+ *
+ * The following is an abstract of the example hereafter.
+ * Please consult the aforemention paper for better UKF-M reference
+ * and the paper Sola-18, [https://arxiv.org/abs/1812.01537] for general
+ * Lie group reference.
+ *
+ *
+ * We consider a robot in the plane surrounded by a small
+ * number of punctual landmarks or _beacons_.
+ * The robot receives control actions in the form of axial
+ * and angular velocities, and is able to measure the location
+ * of the beacons w.r.t its own reference frame.
+ *
+ * The robot pose X is in SE(2) and the beacon positions b_k in R^2,
+ *
+ * | cos th -sin th x |
+ * X = | sin th cos th y | // position and orientation
+ * | 0 0 1 |
+ *
+ * b_k = (bx_k, by_k) // lmk coordinates in world frame
+ *
+ * The control signal u is a twist in se(2) comprising longitudinal
+ * velocity v and angular velocity w, with no lateral velocity
+ * component, integrated over the sampling time dt.
+ *
+ * u = (v*dt, 0, w*dt)
+ *
+ * The control is corrupted by additive Gaussian noise u_noise,
+ * with covariance
+ *
+ * Q = diagonal(sigma_v^2, sigma_s^2, sigma_w^2).
+ *
+ * This noise accounts for possible lateral slippage u_s
+ * through a non-zero value of sigma_s,
+ *
+ * At the arrival of a control u, the robot pose is updated
+ * with X <-- X * Exp(u) = X + u.
+ *
+ * Landmark measurements are of the range and bearing type,
+ * though they are put in Cartesian form for simplicity.
+ * Their noise n is zero mean Gaussian, and is specified
+ * with a covariances matrix R.
+ * We notice the rigid motion action y = h(X,b) = X^-1 * b
+ * (see appendix C),
+ *
+ * y_k = (brx_k, bry_k) // lmk coordinates in robot frame
+ *
+ * We consider the beacons b_k situated at known positions.
+ * We define the pose to estimate as X in SE(2).
+ * The estimation error dx and its covariance P are expressed
+ * in the tangent space at X.
+ *
+ * All these variables are summarized again as follows
+ *
+ * X : robot pose, SE(2)
+ * u : robot control, (v*dt ; 0 ; w*dt) in se(2)
+ * Q : control perturbation covariance
+ * b_k : k-th landmark position, R^2
+ * y : Cartesian landmark measurement in robot frame, R^2
+ * R : covariance of the measurement noise
+ *
+ * The motion and measurement models are
+ *
+ * X_(t+1) = f(X_t, u) = X_t * Exp ( w ) // motion equation
+ * y_k = h(X, b_k) = X^-1 * b_k // measurement equation
+ *
+ * The algorithm below comprises first a simulator to
+ * produce measurements, then uses these measurements
+ * to estimate the state, using a Lie-based error-state Kalman filter.
+ *
+ * This file has plain code with only one main() function.
+ * There are no function calls other than those involving `manif`.
+ *
+ * Printing simulated state and estimated state together
+ * with an unfiltered state (i.e. without Kalman corrections)
+ * allows for evaluating the quality of the estimates.
+ */
+
+#include <iomanip>
+#include <iostream>
+#include <tuple>
+#include <vector>
+
+#include <Eigen/Cholesky>
+
+#include "manif/SE2.h"
+#include <sciplot/sciplot.hpp>
+
+using std::cout;
+using std::endl;
+
+using namespace Eigen;
+
+typedef Array<double, 2, 1> Array2d;
+typedef Array<double, 3, 1> Array3d;
+
+template <typename Scalar>
+struct Weights
+{
+ Weights() = default;
+ ~Weights() = default;
+
+ Weights(const Scalar l, const Scalar alpha)
+ {
+ using std::sqrt;
+
+ const Scalar m = (alpha * alpha - 1) * l;
+ const Scalar ml = m + l;
+
+ sqrt_d_lambda = sqrt(ml);
+ wj = Scalar(1) / (Scalar(2) * (ml));
+ wm = m / (ml);
+ w0 = m / (ml) + Scalar(3) - alpha * alpha;
+ }
+
+ Scalar sqrt_d_lambda;
+ Scalar wj;
+ Scalar wm;
+ Scalar w0;
+};
+
+using Weightsd = Weights<double>;
+
+template <typename Scalar>
+std::tuple<Weights<Scalar>, Weights<Scalar>, Weights<Scalar>>
+compute_sigma_weights(const Scalar state_size,
+ const Scalar propagation_noise_size,
+ const Scalar alpha_0,
+ const Scalar alpha_1,
+ const Scalar alpha_2)
+{
+ assert(state_size > 0);
+ assert(propagation_noise_size > 0);
+ assert(alpha_0 >= 1e-3 && alpha_0 <= 1);
+ assert(alpha_1 >= 1e-3 && alpha_1 <= 1);
+ assert(alpha_2 >= 1e-3 && alpha_2 <= 1);
+
+ return std::make_tuple(Weights<Scalar>(state_size, alpha_0),
+ Weights<Scalar>(propagation_noise_size, alpha_1),
+ Weights<Scalar>(state_size, alpha_2));
+}
+
+int
+main()
+{
+ std::srand((unsigned int)time(0));
+
+ // START CONFIGURATION
+ //
+ //
+ const int NUMBER_OF_LMKS_TO_MEASURE = 3;
+ constexpr int DoF = manif::SE2d::DoF;
+ constexpr int SystemNoiseSize = manif::SE2d::DoF;
+ // Measurement Dim
+ constexpr int Rp = 2;
+
+ // Define the robot pose element and its covariance
+ manif::SE2d X = manif::SE2d::Identity(), X_simulation = manif::SE2d::Identity(),
+ X_unfiltered = manif::SE2d::Identity();
+ Matrix3d P = Matrix3d::Identity() * 1e-6;
+
+ // Define a control vector and its noise and covariance
+ manif::SE2Tangentd u_simu, u_est, u_unfilt;
+ Vector3d u_nom, u_noisy, u_noise;
+ Array3d u_sigmas;
+ Matrix3d U, Uchol;
+
+ u_nom << 0.1, 0.0, 0.05;
+ u_sigmas << 0.1, 0.1, 0.1;
+ U = (u_sigmas * u_sigmas).matrix().asDiagonal();
+ Uchol = U.llt().matrixL();
+
+ // Define three landmarks in R^2
+ Eigen::Vector2d b;
+ const std::vector<Eigen::Vector2d> landmarks{
+ Eigen::Vector2d(2.0, 0.0), Eigen::Vector2d(2.0, 1.0), Eigen::Vector2d(2.0, -1.0)};
+
+ // Define the beacon's measurements
+ Vector2d y, y_bar, y_noise;
+ Matrix<double, Rp, 2 * DoF> yj;
+ Array2d y_sigmas;
+ Matrix2d R;
+ std::vector<Vector2d> measurements(landmarks.size());
+
+ y_sigmas << 0.01, 0.01;
+ R = (y_sigmas * y_sigmas).matrix().asDiagonal();
+
+
+ // Declare UFK variables
+ Array3d alpha;
+ alpha << 1e-3, 1e-3, 1e-3;
+
+ Weightsd w_d, w_q, w_u;
+ std::tie(w_d, w_q, w_u) = compute_sigma_weights<double>(DoF, Rp, alpha(0), alpha(1), alpha(2));
+
+ // Declare some temporaries
+
+ manif::SE2d X_new;
+ Matrix3d P_new;
+ manif::SE2d s_j_p, s_j_m;
+ Vector3d xi_mean;
+ Vector3d w_p, w_m;
+
+ Matrix2d P_yy;
+ Matrix<double, DoF, 2 * DoF> xij;
+ Matrix<double, DoF, 2> P_xiy;
+
+ Vector2d e, z; // expectation, innovation
+ Matrix<double, 3, 2> K; // Kalman gain
+ manif::SE2Tangentd dx; // optimal update step, or error-state
+
+ Matrix<double, DoF, DoF> xis;
+ Matrix<double, DoF, DoF * 2> xis_new;
+ Matrix<double, DoF, SystemNoiseSize * 2> xis_new2;
+
+ //
+ //
+ // CONFIGURATION DONE
+
+
+ // DEBUG
+ cout << std::fixed << std::setprecision(4) << std::showpos << endl;
+ cout << "X STATE : X Y THETA" << endl;
+ cout << "----------------------------------" << endl;
+ cout << "X initial : " << X_simulation.log().coeffs().transpose() << endl;
+ cout << "----------------------------------" << endl;
+ // END DEBUG
+
+
+ std::vector<double> X_simulated_x, X_simulated_y, X_simulated_theta;
+ std::vector<double> X_estimated_x, X_estimated_y, X_estimated_theta;
+ std::vector<double> X_unfiltered_x, X_unfiltered_y, X_unfiltered_theta;
+
+ // START TEMPORAL LOOP
+ //
+ //
+
+ // Make 10 steps. Measure up to three landmarks each time.
+ for (int t = 0; t < 10; t++)
+ {
+ //// I. Simulation ###############################################################################
+
+ /// simulate noise
+ u_noise = u_sigmas * Array3d::Random(); // control noise
+ u_noisy = u_nom + u_noise; // noisy control
+
+ u_simu = u_nom;
+ u_est = u_noisy;
+ u_unfilt = u_noisy;
+
+ /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+ X_simulation = X_simulation + u_simu; // overloaded X.rplus(u) = X * exp(u)
+
+ /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
+ for (std::size_t i = 0; i < landmarks.size(); i++)
+ {
+ b = landmarks[i]; // lmk coordinates in world frame
+
+ /// simulate noise
+ y_noise = y_sigmas * Array2d::Random(); // measurement noise
+
+ y = X_simulation.inverse().act(b); // landmark measurement, before adding noise
+ y = y + y_noise; // landmark measurement, noisy
+ measurements[i] = y; // store for the estimator just below
+ }
+
+
+ //// II. Estimation ###############################################################################
+
+ /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+
+ X_new = X + u_est; // X * exp(u)
+
+ // set sigma points
+ xis = w_d.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();
+
+ // sigma points on manifold
+ for (int i = 0; i < DoF; ++i)
+ {
+ s_j_p = X + manif::SE2Tangentd(xis.col(i));
+ s_j_m = X + manif::SE2Tangentd(-xis.col(i));
+
+ xis_new.col(i) = (X_new.lminus(s_j_p + u_est)).coeffs();
+ xis_new.col(i + DoF) = (X_new.lminus(s_j_m + u_est)).coeffs();
+ }
+
+ // compute covariance
+ xi_mean = w_d.wj * xis_new.rowwise().sum();
+ xis_new.colwise() -= xi_mean;
+
+ P_new = w_d.wj * xis_new * xis_new.transpose() + w_d.w0 * xi_mean * xi_mean.transpose();
+
+ // sigma points on manifold
+ for (int i = 0; i < SystemNoiseSize; ++i)
+ {
+ w_p = w_q.sqrt_d_lambda * Uchol.col(i);
+ w_m = -w_q.sqrt_d_lambda * Uchol.col(i);
+
+ xis_new2.col(i) = (X_new.lminus(X + (u_est + w_p))).coeffs();
+ xis_new2.col(i + SystemNoiseSize) = (X_new.lminus(X + (u_est + w_m))).coeffs();
+ }
+
+ xi_mean = w_q.wj * xis_new2.rowwise().sum();
+ xis_new2.colwise() -= xi_mean;
+
+ U = w_q.wj * xis_new2 * xis_new2.transpose() + w_q.w0 * xi_mean * xi_mean.transpose();
+
+ P = P_new + U;
+
+ X = X_new;
+
+ /// Then we correct using the measurements of each lmk - - - - - - - - -
+ for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
+ {
+ // landmark
+ b = landmarks[i]; // lmk coordinates in world frame
+
+ // measurement
+ y = measurements[i]; // lmk measurement, noisy
+
+ // expectation
+ e = X.inverse().act(b);
+
+ // set sigma points
+ xis = w_u.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();
+
+ // compute measurement sigma points
+ for (int d = 0; d < DoF; ++d)
+ {
+ s_j_p = X + manif::SE2Tangentd(xis.col(d));
+ s_j_m = X + manif::SE2Tangentd(-xis.col(d));
+
+ yj.col(d) = s_j_p.inverse().act(b);
+ yj.col(d + DoF) = s_j_m.inverse().act(b);
+ }
+
+ // measurement mean
+ y_bar = w_u.wm * e + w_u.wj * yj.rowwise().sum();
+
+ yj.colwise() -= y_bar;
+ e -= y_bar;
+
+ // compute covariance and cross covariance matrices
+ P_yy = w_u.w0 * e * e.transpose() + w_u.wj * yj * yj.transpose() + R;
+
+ xij << xis, -xis;
+ P_xiy = w_u.wj * xij * yj.transpose();
+
+ // Kalman gain
+ K = P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
+
+ // innovation
+ z = y - y_bar;
+
+ // Correction step
+ dx = K * z; // dx is in the tangent space at X
+
+ // Update
+ X = X + dx; // overloaded X.rplus(dx) = X * exp(dx)
+ P = P - K * P_yy * K.transpose();
+ }
+
+
+ //// III. Unfiltered ##############################################################################
+
+ // move also an unfiltered version for comparison purposes
+ X_unfiltered = X_unfiltered + u_unfilt;
+
+
+ //// IV. Results ##############################################################################
+
+ // DEBUG
+ cout << "X simulated : " << X_simulation.log().coeffs().transpose() << "\n";
+ cout << "X estimated : " << X.log().coeffs().transpose() << "\n";
+ cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << "\n";
+ cout << "----------------------------------" << endl;
+
+ const auto& X_simu = X_simulation.log().coeffs().transpose();
+ const auto& X_est = X.log().coeffs().transpose();
+ const auto& X_unf = X_unfiltered.log().coeffs().transpose();
+ X_simulated_x.push_back(X_simu(0));
+ X_simulated_y.push_back(X_simu(1));
+ X_simulated_theta.push_back(X_simu(2));
+ X_estimated_x.push_back(X_est(0));
+ X_estimated_y.push_back(X_est(1));
+ X_estimated_theta.push_back(X_est(2));
+ X_unfiltered_x.push_back(X_unf(0));
+ X_unfiltered_y.push_back(X_unf(1));
+ X_unfiltered_theta.push_back(X_unf(2));
+ // END DEBUG
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange("*", "*").yrange("*", "*");
+
+ pos_plot.drawCurve(X_simulated_x, X_simulated_y).label("Simulated");
+ pos_plot.drawCurve(X_estimated_x, X_estimated_y).label("Estimated");
+ pos_plot.drawCurve(X_unfiltered_x, X_unfiltered_y).label("Unfiltered");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange("*", "*").yrange("*", "*");
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, X_estimated_theta.size());
+ orientation_plot.drawCurve(x, X_simulated_theta).label("Simulated");
+ orientation_plot.drawCurve(x, X_estimated_theta).label("Estimated");
+ orientation_plot.drawCurve(x, X_unfiltered_theta).label("Unfiltered");
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 500);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("manif_kalman_output.pdf");
+
+
+ //
+ //
+ // END OF TEMPORAL LOOP. DONE.
+
+ return 0;
+}
diff --git a/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c83f4db5b4fedc49a6af0077229a075ea9e7cd25
--- /dev/null
+++ b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
@@ -0,0 +1,296 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::human
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSE2<T>;
+
+ constexpr T RAD = 10;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T rot_noise_std = 0.1;
+ constexpr T pos_noise_std = 1;
+ constexpr T obs_rot_noise_std = 0.1;
+ constexpr T obs_pos_noise_std = 1;
+ constexpr T initial_offset_angle = 0.1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.1 * RAD * Vector(1, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.pose = manif::SE2<T>(RAD, 0, startAngle);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+
+ const Vector last_pos = states.at(i - 1).pose.translation();
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ Eigen::Vector2d euclidean_velocity = RAD * Eigen::Vector2d{0, c};
+ SystemModelT::ControlT control;
+ //control.velocity.coeffs() << (pos - last_pos) / dt, c;
+ control.velocity.coeffs() << euclidean_velocity, c;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.pose = manif::SE2<T>(pos.x(), pos.y(), angle + startAngle);
+
+ T pos_diff = (propagated.pose.translation() - state.pose.translation()).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.pose.angle() - state.pose.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ states.push_back(propagated);
+
+
+ // add noise
+ control.velocity.coeffs().segment(0, 2) += pos_noise_std * Vector::Random();
+ control.velocity.coeffs().segment(2, 1) +=
+ rot_noise_std * Eigen::Matrix<T, 1, 1>::Random();
+ omegas.push_back(control);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 2) *= obs_pos_noise_std;
+ noise.segment(2, 1) *= obs_rot_noise_std;
+
+ BOOST_TEST_MESSAGE("SALT TEST: noise " << noise);
+
+ observations.push_back(obs + noise);
+
+ BOOST_TEST_MESSAGE("SALT TEST: state:\n"
+ << state.pose.x() << "\n"
+ << state.pose.y() << "\n"
+ << state.pose.angle());
+
+ BOOST_TEST_MESSAGE("SALT TEST: state.log:\n" << state.pose.log().coeffs());
+
+ BOOST_TEST_MESSAGE("SALT TEST: noisy obs:\n" << (obs + noise));
+
+ SystemModelT::ObsT diff =
+ (obs + noise) -
+ SystemModelT::ObsT(state.pose.x(), state.pose.y(), state.pose.angle());
+
+ BOOST_TEST_MESSAGE("\nSALT TEST: diff:\n" << diff);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(se2KalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<2, 2>(0, 0) *= pos_noise_std * pos_noise_std;
+ Q.block<1, 1>(2, 2) *= rot_noise_std * rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= obs_pos_noise_std * obs_pos_noise_std;
+ R.block<1, 1>(2, 2) *= obs_rot_noise_std * obs_rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ P0.block<1, 1>(2, 2) *= initial_offset_angle * initial_offset_angle;
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.5; //0.8; // state
+ alpha(1) = 0.5; //0.1; // control
+ alpha(2) = 0.5; //0.1; // update
+
+ SystemModelT::StateT state0;
+ const Vector startPos = states.at(0).pose.translation() + initial_offet_pos;
+ state0.pose = manif::SE2<T>{
+ startPos.x(), startPos.y(), states.at(0).pose.angle() + initial_offset_angle};
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
+ std::vector<T> a_true, a_obs, a_ukf;
+ std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
+ std::vector<T> ukf_pos_var, ukf_rot_var, ukf_var;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i - 1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 1 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+ BOOST_TEST_MESSAGE("Diag: " << (ukf.getCurrentStateCovariance() -
+ Eigen::Matrix<T, 3, 3>::Identity())
+ .norm());
+
+ manif::SE2Tangent<T> obs{observations.at(i)};
+ x_obs.push_back(obs.exp().x());
+ y_obs.push_back(obs.exp().y());
+ a_obs.push_back(obs.exp().angle());
+
+ x_ukf.push_back(ukf.getCurrentState().pose.x());
+ y_ukf.push_back(ukf.getCurrentState().pose.y());
+ a_ukf.push_back(ukf.getCurrentState().pose.angle());
+
+ TIMING_END(REST);
+ }
+ x_true.push_back(states.at(i).pose.x());
+ y_true.push_back(states.at(i).pose.y());
+ a_true.push_back(states.at(i).pose.angle());
+
+ x_ukf_full.push_back(ukf.getCurrentState().pose.x());
+ y_ukf_full.push_back(ukf.getCurrentState().pose.y());
+ a_ukf_full.push_back(ukf.getCurrentState().pose.angle());
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(0, 0).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(2, 2).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.2, RAD * 1.2).yrange(-RAD * 1.2, RAD * 1.2);
+ //pos_plot.xrange(-4, 4).yrange(-4, 4);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(x, a_ukf).label("UKF").lineWidth(1);
+ orientation_plot.drawCurve(x_t, a_ukf_full).label("Full UKF").lineWidth(1);
+
+
+ sciplot::Plot2D error_plot;
+ error_plot.xlabel("t");
+ error_plot.ylabel("err");
+ error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ error_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_rot_var).label("Rot variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_var).label("Total variance").lineWidth(1);
+
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}, {error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("se2_kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9693e0f83b60b5b93d01a549e4d8ca1e2d4c0f14
--- /dev/null
+++ b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
@@ -0,0 +1,292 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::human
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSE2xV<T>;
+
+ constexpr T RAD = 1;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T obs_rot_noise_std = 0.01;
+ constexpr T obs_pos_noise_std = 0.01;
+ constexpr T initial_offset_angle = 1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.8 * RAD * Vector(1, 0.5);
+ const Eigen::Matrix<T, 3, 1> initial_offet_vel =
+ 0.8 * RAD * Eigen::Matrix<T, 3, 1>(1, 0.5, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.pose = manif::SE2<T>(RAD, 0, startAngle);
+ state.velocity = manif::SE2Tangent<T>(0, c, c);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+
+ const Vector last_pos = states.at(i - 1).pose.translation();
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ SystemModelT::ControlT control;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.pose = manif::SE2<T>(pos.x(), pos.y(), angle + startAngle);
+ state.velocity = manif::SE2Tangent<T>(0, c, c);
+
+ T pos_diff = (propagated.pose.translation() - state.pose.translation()).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.pose.angle() - state.pose.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ T vel_diff = (propagated.velocity.coeffs() - state.velocity.coeffs()).norm();
+ BOOST_TEST(vel_diff < 2e-10,
+ "propagated velocity differs too much from real velocity: " << vel_diff);
+
+ states.push_back(state);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 2) *= obs_pos_noise_std;
+ noise.segment(2, 1) *= obs_rot_noise_std;
+
+ observations.push_back(obs + noise);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(se2xVkalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= obs_pos_noise_std * obs_pos_noise_std;
+ R.block<1, 1>(2, 2) *= obs_rot_noise_std * obs_rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ P0.block<1, 1>(2, 2) *= initial_offset_angle * initial_offset_angle;
+ P0.block<3, 3>(3, 3) *= initial_offet_vel.norm() * initial_offet_vel.norm();
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.001; // state
+ alpha(1) = 0.001; // control
+ alpha(2) = 0.001; // update
+
+ SystemModelT::StateT state0;
+ const Vector startPos = states.at(0).pose.translation() + initial_offet_pos;
+ state0.pose = manif::SE2<T>{
+ startPos.x(), startPos.y(), states.at(0).pose.angle() + initial_offset_angle};
+ state0.velocity = states.at(0).velocity + initial_offet_vel;
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
+ std::vector<T> a_true, a_obs, a_ukf;
+ std::vector<T> vel_x_true, vel_y_true, vel_a_true;
+ std::vector<T> vel_x_ukf_full, vel_y_ukf_full, vel_a_ukf_full;
+ std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
+ std::vector<T> ukf_pos_var, ukf_rot_var, ukf_var;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(SystemModelT::ControlT{}, dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 5 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ x_obs.push_back(observations.at(i)(0));
+ y_obs.push_back(observations.at(i)(1));
+ a_obs.push_back(observations.at(i)(2));
+
+ x_ukf.push_back(ukf.getCurrentState().pose.x());
+ y_ukf.push_back(ukf.getCurrentState().pose.y());
+ a_ukf.push_back(ukf.getCurrentState().pose.angle());
+
+ TIMING_END(REST);
+ }
+
+ x_true.push_back(states.at(i).pose.x());
+ y_true.push_back(states.at(i).pose.y());
+ a_true.push_back(states.at(i).pose.angle());
+
+ vel_x_true.push_back(states.at(i).velocity.x());
+ vel_y_true.push_back(states.at(i).velocity.y());
+ vel_a_true.push_back(states.at(i).velocity.angle());
+
+ x_ukf_full.push_back(ukf.getCurrentState().pose.x());
+ y_ukf_full.push_back(ukf.getCurrentState().pose.y());
+ a_ukf_full.push_back(ukf.getCurrentState().pose.angle());
+
+ vel_x_ukf_full.push_back(ukf.getCurrentState().velocity.x());
+ vel_y_ukf_full.push_back(ukf.getCurrentState().velocity.y());
+ vel_a_ukf_full.push_back(ukf.getCurrentState().velocity.angle());
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(0, 0).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(2, 2).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.1, RAD * 1.1).yrange(-RAD * 1.1, RAD * 1.1);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(x, a_ukf).label("UKF").lineWidth(1);
+ orientation_plot.drawCurve(x_t, a_ukf_full).label("Full UKF").lineWidth(1);
+
+
+ sciplot::Plot2D velocity_plot;
+ velocity_plot.xlabel("t");
+ velocity_plot.ylabel("vel");
+ velocity_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ velocity_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ velocity_plot.drawCurve(x_t, vel_x_true).label("x True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_y_true).label("y True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_a_true).label("a True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_x_ukf_full).label("x Ukf").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_y_ukf_full).label("y Ukf").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_a_ukf_full).label("a Ukf").lineWidth(1);
+
+
+ sciplot::Plot2D error_plot;
+ error_plot.xlabel("t");
+ error_plot.ylabel("err");
+ error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ error_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_rot_var).label("Rot variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_var).label("Total variance").lineWidth(1);
+
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}, {velocity_plot, error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("se2xV_kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp b/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..48529d44139b767010cc920790826bae9936312f
--- /dev/null
+++ b/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp
@@ -0,0 +1,294 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::human
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
+
+ constexpr T RAD = 40;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T rot_noise_std = 0.2;
+ constexpr T pos_noise_std = 5;
+ constexpr T obs_rot_noise_std = 0.2;
+ constexpr T obs_pos_noise_std = 5;
+ constexpr T initial_offset_angle = 1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.8 * RAD * Vector(1, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.position = Vector{RAD, 0};
+ state.orientation = manif::SO2<T>(startAngle);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+ const T last_angle = dt * (i - 1) * c;
+
+ const Vector last_pos = states.at(i - 1).position;
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ SystemModelT::ControlT control;
+ //TODO (SALt): check angular velocity (>pi overflow)
+ control.angular_velocity.coeffs() << c; //(last_angle - angle) / dt;
+ control.euclidean_velocity << (pos - last_pos) / dt;
+ //control.euclidean_velocity = Eigen::Vector2d{0, c};
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.position = pos;
+ state.orientation = manif::SO2<T>(angle + startAngle);
+
+ T pos_diff = (propagated.position - state.position).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.orientation.angle() - state.orientation.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ states.push_back(propagated);
+
+
+ // add noise
+ control.angular_velocity.coeffs() += rot_noise_std * Eigen::Matrix<T, 1, 1>::Random();
+ control.euclidean_velocity += pos_noise_std * Vector::Random();
+ omegas.push_back(control);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+ SystemModelT::ObsT true_obs;
+ true_obs.segment(0, 1) = state.orientation.log().coeffs();
+ true_obs.segment(1, 2) = state.position;
+
+ BOOST_TEST(
+ (obs - true_obs).norm() < std::numeric_limits<T>::epsilon(),
+ "observation differs too much from real observation: " << (obs - true_obs).norm());
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 1) *= obs_rot_noise_std;
+ noise.segment(1, 2) *= obs_pos_noise_std;
+
+ observations.push_back(obs + noise);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(so2KalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<1, 1>(0, 0) *= rot_noise_std * rot_noise_std;
+ Q.block<2, 2>(1, 1) *= pos_noise_std * pos_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<1, 1>(0, 0) *= obs_rot_noise_std * obs_rot_noise_std;
+ R.block<2, 2>(1, 1) *= obs_pos_noise_std * obs_pos_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<1, 1>(0, 0) *= initial_offset_angle * initial_offset_angle;
+ P0.block<2, 2>(1, 1) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.5; // state
+ alpha(1) = 0.5; // control
+ alpha(2) = 0.5; // update
+
+ SystemModelT::StateT state0;
+ state0.orientation = manif::SO2<T>(states.at(0).orientation.angle() + initial_offset_angle);
+ state0.position = states.at(0).position + initial_offet_pos;
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
+ std::vector<T> a_true, a_obs, a_ukf;
+ std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
+ std::vector<T> ukf_pos_var, ukf_rot_var, ukf_var;
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i - 1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 50 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+ const SystemModelT::StateT& current_state = ukf.getCurrentState();
+
+ BOOST_TEST_MESSAGE(
+ "Position Diff: " << (states.at(i).position - current_state.position).norm());
+ BOOST_TEST_MESSAGE(
+ "Orientation Diff: "
+ << (states.at(i).orientation - current_state.orientation).coeffs().norm());
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+ BOOST_TEST_MESSAGE("Diag: " << (ukf.getCurrentStateCovariance() -
+ Eigen::Matrix<T, 3, 3>::Identity())
+ .norm());
+
+ x_obs.push_back(observations.at(i)(1));
+ y_obs.push_back(observations.at(i)(2));
+ a_obs.push_back(observations.at(i)(0));
+
+ x_ukf.push_back(ukf.getCurrentState().position.x());
+ y_ukf.push_back(ukf.getCurrentState().position.y());
+ a_ukf.push_back(ukf.getCurrentState().orientation.log().coeffs()(0));
+
+ TIMING_END(REST);
+ }
+ x_true.push_back(states.at(i).position.x());
+ y_true.push_back(states.at(i).position.y());
+ a_true.push_back(states.at(i).orientation.log().coeffs()(0));
+
+ x_ukf_full.push_back(ukf.getCurrentState().position.x());
+ y_ukf_full.push_back(ukf.getCurrentState().position.y());
+ a_ukf_full.push_back(ukf.getCurrentState().orientation.log().coeffs()(0));
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(1, 1).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(0, 0).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.1, RAD * 1.1).yrange(-RAD * 1.1, RAD * 1.1);
+ //pos_plot.xrange(-4, 4).yrange(-4, 4);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("x");
+ orientation_plot.ylabel("y");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ orientation_plot.drawCurve(x, a_ukf).label("UKF").lineWidth(1);
+ orientation_plot.drawCurve(x_t, a_ukf_full).label("Full UKF").lineWidth(1);
+
+
+ sciplot::Plot2D error_plot;
+ error_plot.xlabel("x");
+ error_plot.ylabel("y");
+ error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ error_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_rot_var).label("Rot variance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_var).label("Total variance").lineWidth(1);
+
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}, {error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("so2_kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::human