Debugging kalman test

Try periodic clamping of observation orientation.
Plot covariance of kalman filter.

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

@@ -25,6 +25,8 @@
 #include <cstdlib> /* srand, rand */
 #include <ctime> /* time */
 
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/util/time.h>
@@ -124,7 +126,24 @@ namespace armarx::navigation::components::dynamic_scene_provider
                 (obs - true_obs).norm() < std::numeric_limits<T>::epsilon(),
                 "observation differs too much from real observation: " << (obs - true_obs).norm());
 
-            observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
+            //observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
+            //continue;
+
+            obs += obs_noise_std * SystemModelT::ObsT::Random();
+
+
+            SystemModelT::StateT noisyState;
+            noisyState.orientation = manif::SO2<T>(obs(0));
+            noisyState.position = obs.segment(1, 2);
+
+            constexpr T eps = 0.1;
+            SystemModelT::ObsT noisyObs = SystemModelT::observationFunction(noisyState);
+
+            //noisyObs(0) = std::clamp(noisyObs(0), -M_PI + eps, M_PI - eps);
+
+            noisyObs(0) = simox::math::periodic_clamp(noisyObs(0), -M_PI, M_PI);
+
+            observations.push_back(noisyObs);
         }
     }
 
@@ -172,6 +191,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
         std::vector<T> a_true, a_obs, a_ukf;
         std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
+        std::vector<T> ukf_pos_var, ukf_rot_var, ukf_var;
 
         for (int i = 1; i < num_timesteps; i++)
         {
@@ -227,6 +247,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
             ukf_states.push_back(ukf.getCurrentState());
             ukf_Ps.push_back(ukf.getCurrentStateCovariance());
 
+            ukf_pos_var.push_back(
+                ukf.getCurrentStateCovariance().block<2, 2>(1, 1).cwiseAbs().maxCoeff());
+            ukf_rot_var.push_back(
+                ukf.getCurrentStateCovariance().block<1, 1>(0, 0).cwiseAbs().maxCoeff());
+            ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
             TIMING_END(LOOP);
         }
 
@@ -257,12 +283,22 @@ namespace armarx::navigation::components::dynamic_scene_provider
         orientation_plot.drawCurve(x, a_ukf).label("UKF").lineWidth(1);
         orientation_plot.drawCurve(x_t, a_ukf_full).label("Full UKF").lineWidth(1);
 
+        sciplot::Plot2D error_plot;
+        error_plot.xlabel("x");
+        error_plot.ylabel("y");
+        error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+        error_plot.xrange(-0.1, 1.1).yrange(-1e-3, 1e-3);
+
+        error_plot.drawCurve(x_t, ukf_pos_var).label("Pos cariance").lineWidth(1);
+        error_plot.drawCurve(x_t, ukf_rot_var).label("Rot variance").lineWidth(1);
+        error_plot.drawCurve(x_t, ukf_var).label("Total variance").lineWidth(1);
+
 
         // Create figure to hold plot
-        sciplot::Figure fig = {{pos_plot}, {orientation_plot}};
+        sciplot::Figure fig = {{pos_plot, orientation_plot}, {error_plot}};
         // Create canvas to hold figure
         sciplot::Canvas canvas = {{fig}};
-        canvas.size(600, 1200);
+        canvas.size(1000, 1000);
 
         // Show the plot in a pop-up window
         canvas.show();