Move HumanSystemModel to human library

source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt

@@ -10,13 +10,10 @@ armarx_add_component(dynamic_scene_provider
         Component.cpp
         ArVizDrawer.cpp
         HumanTracker.cpp
-        HumanSystemModel.cpp
-        UnscentedKalmanFilter_impl.cpp
     HEADERS
         Component.h
         ArVizDrawer.h
         HumanTracker.h
-        HumanSystemModel.h
     DEPENDENCIES
         # ArmarXCore
         ArmarXCore
@@ -24,7 +21,6 @@ armarx_add_component(dynamic_scene_provider
         # ArmarXGui
         ## ArmarXGuiComponentPlugins  # For RemoteGui plugin.
         # RobotAPI
-        ukfm
         armem
         armem_robot
         armem_robot_state
@@ -54,4 +50,5 @@ armarx_add_test(kalman_test
     DEPENDENCIES
         PUBLIC
             ArmarXCore
+            armarx_navigation::teb_human
 )

source/armarx/navigation/human/CMakeLists.txt

@@ -11,15 +11,19 @@ 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
     HEADERS
         types.h
         aron_conversions.h
         shapes.h
         ProxemicZoneCreator.h
+        HumanSystemModel.h
 )

source/armarx/navigation/components/dynamic_scene_provider/HumanSystemModel.cpp → source/armarx/navigation/human/HumanSystemModel.cpp

@@ -10,8 +10,8 @@ namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
     SystemModelSO2xR2<floatT>::observationFunction(const SystemModelSO2xR2::StateT& state)
     {
         ObsT observation;
-        observation.segment(0, 2) = state.position;
-        observation.segment(2, 1) = state.orientation.log().coeffs();
+        observation.segment(0, 1) = state.orientation.log().coeffs();
+        observation.segment(1, 2) = state.position;
         return observation;
     }
 

source/armarx/navigation/components/dynamic_scene_provider/HumanSystemModel.h → source/armarx/navigation/human/HumanSystemModel.h

@@ -21,8 +21,6 @@
  */
 #pragma once
 
-#include <RobotAPI/libraries/ukfm/SystemModel.h>
-
 #include <manif/SO2.h>
 
 namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter