Draft: Implement basic version of kalman filter for human tracking

Implement ukfm from armarx/RobotAPI/libraries

source/armarx/navigation/human/test/kalmanFilterTest.cpp

@@ -81,6 +81,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
             //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);