Add SE2xV system model

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

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