From 537c8b70c1ac3cc09b1a2f70e2fb91009205c58e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 3 Sep 2022 23:27:27 +0200
Subject: [PATCH] Add SE2xV system model
---
.../navigation/human/HumanSystemModel.cpp | 55 +++++++++++++++++++
.../navigation/human/HumanSystemModel.h | 47 ++++++++++++++++
2 files changed, 102 insertions(+)
diff --git a/source/armarx/navigation/human/HumanSystemModel.cpp b/source/armarx/navigation/human/HumanSystemModel.cpp
index 2aabff49..32b18fd1 100644
--- a/source/armarx/navigation/human/HumanSystemModel.cpp
+++ b/source/armarx/navigation/human/HumanSystemModel.cpp
@@ -113,4 +113,59 @@ namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
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::components::dynamic_scene_provider::kalman_filter
diff --git a/source/armarx/navigation/human/HumanSystemModel.h b/source/armarx/navigation/human/HumanSystemModel.h
index fb0309e1..8cbda525 100644
--- a/source/armarx/navigation/human/HumanSystemModel.h
+++ b/source/armarx/navigation/human/HumanSystemModel.h
@@ -121,4 +121,51 @@ namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
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 = 0, 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::components::dynamic_scene_provider::kalman_filter
--
GitLab