From 11a5e219e602abb69c03d6de7760c116621e42df Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 29 Aug 2022 20:05:28 +0200
Subject: [PATCH] Debugging kalman test
Try periodic clamping of observation orientation.
Plot covariance of kalman filter.
---
.../human/test/kalmanFilterTest.cpp | 42 +++++++++++++++++--
1 file changed, 39 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/human/test/kalmanFilterTest.cpp b/source/armarx/navigation/human/test/kalmanFilterTest.cpp
index 1ccc3114..22e37372 100644
--- a/source/armarx/navigation/human/test/kalmanFilterTest.cpp
+++ b/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();
--
GitLab