Adjust so2 kalman test parameters

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

@@ -49,9 +49,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
     constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
 
     constexpr T rot_noise_std = 0.2;
-    constexpr T pos_noise_std = 200;
+    constexpr T pos_noise_std = 5;
     constexpr T obs_rot_noise_std = 0.2;
-    constexpr T obs_pos_noise_std = 200;
+    constexpr T obs_pos_noise_std = 5;
     constexpr T initial_offset_angle = 1 * 10 * M_PI / 180;
     const Vector initial_offet_pos = 0.8 * RAD * Vector(1, 0.5);
 
@@ -163,9 +163,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
         P0.block<2, 2>(1, 1) *= initial_offet_pos.norm() * initial_offet_pos.norm();
 
         UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
-        alpha(0) = 0.8; // state
-        alpha(1) = 0.1; // control
-        alpha(2) = 0.1; // update
+        alpha(0) = 0.5; // state
+        alpha(1) = 0.5; // control
+        alpha(2) = 0.5; // update
 
         SystemModelT::StateT state0;
         state0.orientation = manif::SO2<T>(states.at(0).orientation.angle() + initial_offset_angle);
@@ -285,10 +285,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
         canvas.size(1000, 1000);
 
         // Show the plot in a pop-up window
-        //canvas.show();
+        canvas.show();
 
         // Save the plot to a PDF file
-        canvas.save("kalman_output.pdf");
+        canvas.save("so2_kalman_output.pdf");
     }
 
 } // namespace armarx::navigation::components::dynamic_scene_provider