Kalman test check angular velocity

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

@@ -78,7 +78,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
                        "position differs too much from last step: " << (last_pos - pos).norm());
 
             SystemModelT::ControlT control;
-            control.angular_velocity.coeffs() << (last_angle - angle) / dt;
+            //TODO (SALt): check angular velocity (>pi overflow)
+            control.angular_velocity.coeffs() << c; //(last_angle - angle) / dt;
             control.euclidean_velocity << (pos - last_pos) / dt;
 
             const SystemModelT::StateT propagated = SystemModelT::propagationFunction(