From b30861a097a3eda09149745e8c993ec7b6c63273 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 5 Sep 2022 19:17:10 +0200
Subject: [PATCH] Some kalman filter updates
---
source/armarx/navigation/human/CMakeLists.txt | 35 +-
.../human/UnscentedKalmanFilter_impl.cpp | 3 +
.../navigation/human/test/manifKalmanTest.cpp | 462 ++++++++++++++++++
.../human/test/se2KalmanFilterTest.cpp | 295 +++++++++++
.../human/test/se2xVkalmanFilterTest.cpp | 295 +++++++++++
...FilterTest.cpp => so2kalmanFilterTest.cpp} | 11 +-
6 files changed, 1094 insertions(+), 7 deletions(-)
create mode 100644 source/armarx/navigation/human/test/manifKalmanTest.cpp
create mode 100644 source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
create mode 100644 source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
rename source/armarx/navigation/human/test/{kalmanFilterTest.cpp => so2kalmanFilterTest.cpp} (98%)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index f3b7cc38..2ffaa3d1 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -30,9 +30,9 @@ armarx_add_library(teb_human
find_package(sciplot)
-armarx_add_test(kalman_test
+armarx_add_test(so2_kalman_test
TEST_FILES
- test/kalmanFilterTest.cpp
+ test/so2kalmanFilterTest.cpp
DEPENDENCIES
PUBLIC
ArmarXCore
@@ -40,6 +40,37 @@ armarx_add_test(kalman_test
sciplot::sciplot
)
+armarx_add_test(se2_kalman_test
+ TEST_FILES
+ test/se2KalmanFilterTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+)
+
+armarx_add_test(se2xV_kalman_test
+ TEST_FILES
+ test/se2xVkalmanFilterTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+)
+
+armarx_add_test(manif_kalman_test
+ TEST_FILES
+ test/manifKalmanTest.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::teb_human
+ sciplot::sciplot
+)
+
+
armarx_add_test(se3_kalman_test
TEST_FILES
test/UnscentedKalmanFilterTest.cpp
diff --git a/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
index 672e487f..168a9e03 100644
--- a/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
+++ b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
@@ -7,3 +7,6 @@ template class UnscentedKalmanFilter<armarx::navigation::components::dynamic_sce
kalman_filter::SystemModelSO2xR2<float>>;
template class UnscentedKalmanFilter<armarx::navigation::components::dynamic_scene_provider::
kalman_filter::SystemModelSO2xR2<double>>;
+
+template class UnscentedKalmanFilter<
+ armarx::navigation::components::dynamic_scene_provider::kalman_filter::SystemModelSE2<double>>;
diff --git a/source/armarx/navigation/human/test/manifKalmanTest.cpp b/source/armarx/navigation/human/test/manifKalmanTest.cpp
new file mode 100644
index 00000000..4940ab7c
--- /dev/null
+++ b/source/armarx/navigation/human/test/manifKalmanTest.cpp
@@ -0,0 +1,462 @@
+/**
+ * \file se2_localization_ukfm.cpp
+ *
+ * Created on: Dec 10, 2018
+ * \author: artivis
+ *
+ * ---------------------------------------------------------
+ * This file is:
+ * (c) 2021 Jeremie Deray
+ *
+ * This file is part of `manif`, a C++ template-only library
+ * for Lie theory targeted at estimation for robotics.
+ * Manif is:
+ * (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
+ * ---------------------------------------------------------
+ *
+ * ---------------------------------------------------------
+ * Demonstration example:
+ *
+ * 2D Robot localization based on fixed beacons.
+ *
+ * See se3_localization.cpp for the 3D equivalent.
+ * See se3_sam.cpp for a more advanced example performing smoothing and mapping.
+ * ---------------------------------------------------------
+ *
+ * This demo showcases an application of an Unscented Kalman Filter on Manifold,
+ * based on the paper
+ * 'A Code for Unscented Kalman Filtering on Manifolds (UKF-M)'
+ * [https://arxiv.org/pdf/2002.00878.pdf], M. Brossard, A. Barrau and S. Bonnabel.
+ *
+ * The following is an abstract of the example hereafter.
+ * Please consult the aforemention paper for better UKF-M reference
+ * and the paper Sola-18, [https://arxiv.org/abs/1812.01537] for general
+ * Lie group reference.
+ *
+ *
+ * We consider a robot in the plane surrounded by a small
+ * number of punctual landmarks or _beacons_.
+ * The robot receives control actions in the form of axial
+ * and angular velocities, and is able to measure the location
+ * of the beacons w.r.t its own reference frame.
+ *
+ * The robot pose X is in SE(2) and the beacon positions b_k in R^2,
+ *
+ * | cos th -sin th x |
+ * X = | sin th cos th y | // position and orientation
+ * | 0 0 1 |
+ *
+ * b_k = (bx_k, by_k) // lmk coordinates in world frame
+ *
+ * The control signal u is a twist in se(2) comprising longitudinal
+ * velocity v and angular velocity w, with no lateral velocity
+ * component, integrated over the sampling time dt.
+ *
+ * u = (v*dt, 0, w*dt)
+ *
+ * The control is corrupted by additive Gaussian noise u_noise,
+ * with covariance
+ *
+ * Q = diagonal(sigma_v^2, sigma_s^2, sigma_w^2).
+ *
+ * This noise accounts for possible lateral slippage u_s
+ * through a non-zero value of sigma_s,
+ *
+ * At the arrival of a control u, the robot pose is updated
+ * with X <-- X * Exp(u) = X + u.
+ *
+ * Landmark measurements are of the range and bearing type,
+ * though they are put in Cartesian form for simplicity.
+ * Their noise n is zero mean Gaussian, and is specified
+ * with a covariances matrix R.
+ * We notice the rigid motion action y = h(X,b) = X^-1 * b
+ * (see appendix C),
+ *
+ * y_k = (brx_k, bry_k) // lmk coordinates in robot frame
+ *
+ * We consider the beacons b_k situated at known positions.
+ * We define the pose to estimate as X in SE(2).
+ * The estimation error dx and its covariance P are expressed
+ * in the tangent space at X.
+ *
+ * All these variables are summarized again as follows
+ *
+ * X : robot pose, SE(2)
+ * u : robot control, (v*dt ; 0 ; w*dt) in se(2)
+ * Q : control perturbation covariance
+ * b_k : k-th landmark position, R^2
+ * y : Cartesian landmark measurement in robot frame, R^2
+ * R : covariance of the measurement noise
+ *
+ * The motion and measurement models are
+ *
+ * X_(t+1) = f(X_t, u) = X_t * Exp ( w ) // motion equation
+ * y_k = h(X, b_k) = X^-1 * b_k // measurement equation
+ *
+ * The algorithm below comprises first a simulator to
+ * produce measurements, then uses these measurements
+ * to estimate the state, using a Lie-based error-state Kalman filter.
+ *
+ * This file has plain code with only one main() function.
+ * There are no function calls other than those involving `manif`.
+ *
+ * Printing simulated state and estimated state together
+ * with an unfiltered state (i.e. without Kalman corrections)
+ * allows for evaluating the quality of the estimates.
+ */
+
+#include <iomanip>
+#include <iostream>
+#include <tuple>
+#include <vector>
+
+#include <Eigen/Cholesky>
+
+#include "manif/SE2.h"
+#include <sciplot/sciplot.hpp>
+
+using std::cout;
+using std::endl;
+
+using namespace Eigen;
+
+typedef Array<double, 2, 1> Array2d;
+typedef Array<double, 3, 1> Array3d;
+
+template <typename Scalar>
+struct Weights
+{
+ Weights() = default;
+ ~Weights() = default;
+
+ Weights(const Scalar l, const Scalar alpha)
+ {
+ using std::sqrt;
+
+ const Scalar m = (alpha * alpha - 1) * l;
+ const Scalar ml = m + l;
+
+ sqrt_d_lambda = sqrt(ml);
+ wj = Scalar(1) / (Scalar(2) * (ml));
+ wm = m / (ml);
+ w0 = m / (ml) + Scalar(3) - alpha * alpha;
+ }
+
+ Scalar sqrt_d_lambda;
+ Scalar wj;
+ Scalar wm;
+ Scalar w0;
+};
+
+using Weightsd = Weights<double>;
+
+template <typename Scalar>
+std::tuple<Weights<Scalar>, Weights<Scalar>, Weights<Scalar>>
+compute_sigma_weights(const Scalar state_size,
+ const Scalar propagation_noise_size,
+ const Scalar alpha_0,
+ const Scalar alpha_1,
+ const Scalar alpha_2)
+{
+ assert(state_size > 0);
+ assert(propagation_noise_size > 0);
+ assert(alpha_0 >= 1e-3 && alpha_0 <= 1);
+ assert(alpha_1 >= 1e-3 && alpha_1 <= 1);
+ assert(alpha_2 >= 1e-3 && alpha_2 <= 1);
+
+ return std::make_tuple(Weights<Scalar>(state_size, alpha_0),
+ Weights<Scalar>(propagation_noise_size, alpha_1),
+ Weights<Scalar>(state_size, alpha_2));
+}
+
+int
+main()
+{
+ std::srand((unsigned int)time(0));
+
+ // START CONFIGURATION
+ //
+ //
+ const int NUMBER_OF_LMKS_TO_MEASURE = 3;
+ constexpr int DoF = manif::SE2d::DoF;
+ constexpr int SystemNoiseSize = manif::SE2d::DoF;
+ // Measurement Dim
+ constexpr int Rp = 2;
+
+ // Define the robot pose element and its covariance
+ manif::SE2d X = manif::SE2d::Identity(), X_simulation = manif::SE2d::Identity(),
+ X_unfiltered = manif::SE2d::Identity();
+ Matrix3d P = Matrix3d::Identity() * 1e-6;
+
+ // Define a control vector and its noise and covariance
+ manif::SE2Tangentd u_simu, u_est, u_unfilt;
+ Vector3d u_nom, u_noisy, u_noise;
+ Array3d u_sigmas;
+ Matrix3d U, Uchol;
+
+ u_nom << 0.1, 0.0, 0.05;
+ u_sigmas << 0.1, 0.1, 0.1;
+ U = (u_sigmas * u_sigmas).matrix().asDiagonal();
+ Uchol = U.llt().matrixL();
+
+ // Define three landmarks in R^2
+ Eigen::Vector2d b;
+ const std::vector<Eigen::Vector2d> landmarks{
+ Eigen::Vector2d(2.0, 0.0), Eigen::Vector2d(2.0, 1.0), Eigen::Vector2d(2.0, -1.0)};
+
+ // Define the beacon's measurements
+ Vector2d y, y_bar, y_noise;
+ Matrix<double, Rp, 2 * DoF> yj;
+ Array2d y_sigmas;
+ Matrix2d R;
+ std::vector<Vector2d> measurements(landmarks.size());
+
+ y_sigmas << 0.01, 0.01;
+ R = (y_sigmas * y_sigmas).matrix().asDiagonal();
+
+
+ // Declare UFK variables
+ Array3d alpha;
+ alpha << 1e-3, 1e-3, 1e-3;
+
+ Weightsd w_d, w_q, w_u;
+ std::tie(w_d, w_q, w_u) = compute_sigma_weights<double>(DoF, Rp, alpha(0), alpha(1), alpha(2));
+
+ // Declare some temporaries
+
+ manif::SE2d X_new;
+ Matrix3d P_new;
+ manif::SE2d s_j_p, s_j_m;
+ Vector3d xi_mean;
+ Vector3d w_p, w_m;
+
+ Matrix2d P_yy;
+ Matrix<double, DoF, 2 * DoF> xij;
+ Matrix<double, DoF, 2> P_xiy;
+
+ Vector2d e, z; // expectation, innovation
+ Matrix<double, 3, 2> K; // Kalman gain
+ manif::SE2Tangentd dx; // optimal update step, or error-state
+
+ Matrix<double, DoF, DoF> xis;
+ Matrix<double, DoF, DoF * 2> xis_new;
+ Matrix<double, DoF, SystemNoiseSize * 2> xis_new2;
+
+ //
+ //
+ // CONFIGURATION DONE
+
+
+ // DEBUG
+ cout << std::fixed << std::setprecision(4) << std::showpos << endl;
+ cout << "X STATE : X Y THETA" << endl;
+ cout << "----------------------------------" << endl;
+ cout << "X initial : " << X_simulation.log().coeffs().transpose() << endl;
+ cout << "----------------------------------" << endl;
+ // END DEBUG
+
+
+ std::vector<double> X_simulated_x, X_simulated_y, X_simulated_theta;
+ std::vector<double> X_estimated_x, X_estimated_y, X_estimated_theta;
+ std::vector<double> X_unfiltered_x, X_unfiltered_y, X_unfiltered_theta;
+
+ // START TEMPORAL LOOP
+ //
+ //
+
+ // Make 10 steps. Measure up to three landmarks each time.
+ for (int t = 0; t < 10; t++)
+ {
+ //// I. Simulation ###############################################################################
+
+ /// simulate noise
+ u_noise = u_sigmas * Array3d::Random(); // control noise
+ u_noisy = u_nom + u_noise; // noisy control
+
+ u_simu = u_nom;
+ u_est = u_noisy;
+ u_unfilt = u_noisy;
+
+ /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+ X_simulation = X_simulation + u_simu; // overloaded X.rplus(u) = X * exp(u)
+
+ /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
+ for (std::size_t i = 0; i < landmarks.size(); i++)
+ {
+ b = landmarks[i]; // lmk coordinates in world frame
+
+ /// simulate noise
+ y_noise = y_sigmas * Array2d::Random(); // measurement noise
+
+ y = X_simulation.inverse().act(b); // landmark measurement, before adding noise
+ y = y + y_noise; // landmark measurement, noisy
+ measurements[i] = y; // store for the estimator just below
+ }
+
+
+ //// II. Estimation ###############################################################################
+
+ /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+
+ X_new = X + u_est; // X * exp(u)
+
+ // set sigma points
+ xis = w_d.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();
+
+ // sigma points on manifold
+ for (int i = 0; i < DoF; ++i)
+ {
+ s_j_p = X + manif::SE2Tangentd(xis.col(i));
+ s_j_m = X + manif::SE2Tangentd(-xis.col(i));
+
+ xis_new.col(i) = (X_new.lminus(s_j_p + u_est)).coeffs();
+ xis_new.col(i + DoF) = (X_new.lminus(s_j_m + u_est)).coeffs();
+ }
+
+ // compute covariance
+ xi_mean = w_d.wj * xis_new.rowwise().sum();
+ xis_new.colwise() -= xi_mean;
+
+ P_new = w_d.wj * xis_new * xis_new.transpose() + w_d.w0 * xi_mean * xi_mean.transpose();
+
+ // sigma points on manifold
+ for (int i = 0; i < SystemNoiseSize; ++i)
+ {
+ w_p = w_q.sqrt_d_lambda * Uchol.col(i);
+ w_m = -w_q.sqrt_d_lambda * Uchol.col(i);
+
+ xis_new2.col(i) = (X_new.lminus(X + (u_est + w_p))).coeffs();
+ xis_new2.col(i + SystemNoiseSize) = (X_new.lminus(X + (u_est + w_m))).coeffs();
+ }
+
+ xi_mean = w_q.wj * xis_new2.rowwise().sum();
+ xis_new2.colwise() -= xi_mean;
+
+ U = w_q.wj * xis_new2 * xis_new2.transpose() + w_q.w0 * xi_mean * xi_mean.transpose();
+
+ P = P_new + U;
+
+ X = X_new;
+
+ /// Then we correct using the measurements of each lmk - - - - - - - - -
+ for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
+ {
+ // landmark
+ b = landmarks[i]; // lmk coordinates in world frame
+
+ // measurement
+ y = measurements[i]; // lmk measurement, noisy
+
+ // expectation
+ e = X.inverse().act(b);
+
+ // set sigma points
+ xis = w_u.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();
+
+ // compute measurement sigma points
+ for (int d = 0; d < DoF; ++d)
+ {
+ s_j_p = X + manif::SE2Tangentd(xis.col(d));
+ s_j_m = X + manif::SE2Tangentd(-xis.col(d));
+
+ yj.col(d) = s_j_p.inverse().act(b);
+ yj.col(d + DoF) = s_j_m.inverse().act(b);
+ }
+
+ // measurement mean
+ y_bar = w_u.wm * e + w_u.wj * yj.rowwise().sum();
+
+ yj.colwise() -= y_bar;
+ e -= y_bar;
+
+ // compute covariance and cross covariance matrices
+ P_yy = w_u.w0 * e * e.transpose() + w_u.wj * yj * yj.transpose() + R;
+
+ xij << xis, -xis;
+ P_xiy = w_u.wj * xij * yj.transpose();
+
+ // Kalman gain
+ K = P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
+
+ // innovation
+ z = y - y_bar;
+
+ // Correction step
+ dx = K * z; // dx is in the tangent space at X
+
+ // Update
+ X = X + dx; // overloaded X.rplus(dx) = X * exp(dx)
+ P = P - K * P_yy * K.transpose();
+ }
+
+
+ //// III. Unfiltered ##############################################################################
+
+ // move also an unfiltered version for comparison purposes
+ X_unfiltered = X_unfiltered + u_unfilt;
+
+
+ //// IV. Results ##############################################################################
+
+ // DEBUG
+ cout << "X simulated : " << X_simulation.log().coeffs().transpose() << "\n";
+ cout << "X estimated : " << X.log().coeffs().transpose() << "\n";
+ cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << "\n";
+ cout << "----------------------------------" << endl;
+
+ const auto& X_simu = X_simulation.log().coeffs().transpose();
+ const auto& X_est = X.log().coeffs().transpose();
+ const auto& X_unf = X_unfiltered.log().coeffs().transpose();
+ X_simulated_x.push_back(X_simu(0));
+ X_simulated_y.push_back(X_simu(1));
+ X_simulated_theta.push_back(X_simu(2));
+ X_estimated_x.push_back(X_est(0));
+ X_estimated_y.push_back(X_est(1));
+ X_estimated_theta.push_back(X_est(2));
+ X_unfiltered_x.push_back(X_unf(0));
+ X_unfiltered_y.push_back(X_unf(1));
+ X_unfiltered_theta.push_back(X_unf(2));
+ // END DEBUG
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange("*", "*").yrange("*", "*");
+
+ pos_plot.drawCurve(X_simulated_x, X_simulated_y).label("Simulated");
+ pos_plot.drawCurve(X_estimated_x, X_estimated_y).label("Estimated");
+ pos_plot.drawCurve(X_unfiltered_x, X_unfiltered_y).label("Unfiltered");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange("*", "*").yrange("*", "*");
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, X_estimated_theta.size());
+ orientation_plot.drawCurve(x, X_simulated_theta).label("Simulated");
+ orientation_plot.drawCurve(x, X_estimated_theta).label("Estimated");
+ orientation_plot.drawCurve(x, X_unfiltered_theta).label("Unfiltered");
+
+ // Create figure to hold plot
+ sciplot::Figure fig = {{pos_plot, orientation_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 500);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("manif_kalman_output.pdf");
+
+
+ //
+ //
+ // END OF TEMPORAL LOOP. DONE.
+
+ return 0;
+}
diff --git a/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
new file mode 100644
index 00000000..61d37530
--- /dev/null
+++ b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
@@ -0,0 +1,295 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSE2<T>;
+
+ constexpr T RAD = 1;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T rot_noise_std = 0.1;
+ constexpr T pos_noise_std = 0.1;
+ constexpr T obs_rot_noise_std = 0.1;
+ constexpr T obs_pos_noise_std = 0.1;
+ constexpr T initial_offset_angle = 1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.4 * RAD * Vector(1, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.pose = manif::SE2<T>(RAD, 0, startAngle);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+
+ const Vector last_pos = states.at(i - 1).pose.translation();
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ Eigen::Vector2d euclidean_velocity = Eigen::Vector2d{0, c};
+ SystemModelT::ControlT control;
+ //control.velocity.coeffs() << (pos - last_pos) / dt, c;
+ control.velocity.coeffs() << euclidean_velocity, c;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.pose = manif::SE2<T>(pos.x(), pos.y(), angle + startAngle);
+
+ T pos_diff = (propagated.pose.translation() - state.pose.translation()).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.pose.angle() - state.pose.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ states.push_back(propagated);
+
+
+ // add noise
+ control.velocity.coeffs().segment(0, 2) += pos_noise_std * Vector::Random();
+ control.velocity.coeffs().segment(2, 1) +=
+ rot_noise_std * Eigen::Matrix<T, 1, 1>::Random();
+ omegas.push_back(control);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 2) *= obs_pos_noise_std;
+ noise.segment(2, 1) *= obs_rot_noise_std;
+
+ BOOST_TEST_MESSAGE("SALT TEST: noise " << noise);
+
+ observations.push_back(obs + noise);
+
+ BOOST_TEST_MESSAGE("SALT TEST: state:\n"
+ << state.pose.x() << "\n"
+ << state.pose.y() << "\n"
+ << state.pose.angle());
+
+ BOOST_TEST_MESSAGE("SALT TEST: state.log:\n" << state.pose.log().coeffs());
+
+ BOOST_TEST_MESSAGE("SALT TEST: noisy obs:\n" << (obs + noise));
+
+ SystemModelT::ObsT diff =
+ (obs + noise) -
+ SystemModelT::ObsT(state.pose.x(), state.pose.y(), state.pose.angle());
+
+ BOOST_TEST_MESSAGE("\nSALT TEST: diff:\n" << diff);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(se2KalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ControlT> omegas;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states, omegas);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<2, 2>(0, 0) *= pos_noise_std * pos_noise_std;
+ Q.block<1, 1>(2, 2) *= rot_noise_std * rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= obs_pos_noise_std * obs_pos_noise_std;
+ R.block<1, 1>(2, 2) *= obs_rot_noise_std * obs_rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ P0.block<1, 1>(2, 2) *= initial_offset_angle * initial_offset_angle;
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.5; //0.8; // state
+ alpha(1) = 0.5; //0.1; // control
+ alpha(2) = 0.5; //0.1; // update
+
+ SystemModelT::StateT state0;
+ const Vector startPos = states.at(0).pose.translation() + initial_offet_pos;
+ state0.pose = manif::SE2<T>{
+ startPos.x(), startPos.y(), states.at(0).pose.angle() + initial_offset_angle};
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ 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++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(omegas.at(i - 1), dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 50 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+ BOOST_TEST_MESSAGE("Diag: " << (ukf.getCurrentStateCovariance() -
+ Eigen::Matrix<T, 3, 3>::Identity())
+ .norm());
+
+ x_obs.push_back(observations.at(i)(0));
+ y_obs.push_back(observations.at(i)(1));
+ a_obs.push_back(observations.at(i)(2));
+
+ x_ukf.push_back(ukf.getCurrentState().pose.x());
+ y_ukf.push_back(ukf.getCurrentState().pose.y());
+ a_ukf.push_back(ukf.getCurrentState().pose.angle());
+
+ TIMING_END(REST);
+ }
+ x_true.push_back(states.at(i).pose.x());
+ y_true.push_back(states.at(i).pose.y());
+ a_true.push_back(states.at(i).pose.angle());
+
+ x_ukf_full.push_back(ukf.getCurrentState().pose.x());
+ y_ukf_full.push_back(ukf.getCurrentState().pose.y());
+ a_ukf_full.push_back(ukf.getCurrentState().pose.angle());
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(0, 0).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(2, 2).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.1, RAD * 1.1).yrange(-RAD * 1.1, RAD * 1.1);
+ //pos_plot.xrange(-4, 4).yrange(-4, 4);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("x");
+ orientation_plot.ylabel("y");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ 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("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").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}, {error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("se2_kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
new file mode 100644
index 00000000..5187e87a
--- /dev/null
+++ b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
@@ -0,0 +1,295 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/util/time.h>
+
+#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/human/HumanSystemModel.h>
+#include <manif/SO2.h>
+#include <sciplot/sciplot.hpp>
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ using T = double; //TODO double or float?
+ using Vector = Eigen::Matrix<T, 2, 1>;
+ using SystemModelT = kalman_filter::SystemModelSE2xV<T>;
+
+ constexpr T RAD = 1;
+ constexpr long num_timesteps = 3000;
+ constexpr T max_time = 1;
+ constexpr T dt = max_time / num_timesteps;
+ constexpr T c = (1 / max_time) * 2 * M_PI; // rotation per time
+
+ constexpr T obs_rot_noise_std = 0.01;
+ constexpr T obs_pos_noise_std = 0.01;
+ constexpr T initial_offset_angle = 1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.8 * RAD * Vector(1, 0.5);
+ const Vector initial_offet_vel = 0.8 * RAD * Vector(1, 0.5);
+
+
+ void
+ simulate_trajectory(std::vector<SystemModelT::StateT>& states)
+ {
+ constexpr T startAngle = 0;
+
+ SystemModelT::StateT state;
+ state.pose = manif::SE2<T>(RAD, 0, startAngle);
+ state.velocity = manif::SE2Tangent<T>(0, c, c);
+ states.push_back(state);
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
+ const T t = dt * i;
+ const T angle = t * c;
+
+ const Vector last_pos = states.at(i - 1).pose.translation();
+ //const Vector pos(angle, std::cos(angle));
+ const Vector pos = RAD * Vector(std::cos(angle), std::sin(angle));
+
+ BOOST_TEST(((last_pos - pos).norm() < 0.1 * RAD),
+ "position differs too much from last step: " << (last_pos - pos).norm());
+
+ SystemModelT::ControlT control;
+
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+
+ state.pose = manif::SE2<T>(pos.x(), pos.y(), angle + startAngle);
+ state.velocity = manif::SE2Tangent<T>(0, c, c);
+
+ T pos_diff = (propagated.pose.translation() - state.pose.translation()).norm();
+ BOOST_TEST(pos_diff < 2e-10,
+ "propagated position differs too much from real position: " << pos_diff);
+
+ T rot_diff = propagated.pose.angle() - state.pose.angle();
+ BOOST_TEST(rot_diff < 2e-10,
+ "propagated rotation differs too much from real orientation: " << rot_diff);
+
+ T vel_diff = (propagated.velocity.coeffs() - state.velocity.coeffs()).norm();
+ BOOST_TEST(vel_diff < 2e-10,
+ "propagated velocity differs too much from real velocity: " << vel_diff);
+
+ states.push_back(state);
+ }
+ }
+
+ void
+ simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+ {
+ for (const auto& state : states)
+ {
+ SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
+
+ SystemModelT::ObsT noise = SystemModelT::ObsT::Random();
+ noise.segment(0, 2) *= obs_pos_noise_std;
+ noise.segment(2, 1) *= obs_rot_noise_std;
+
+ observations.push_back(obs + noise);
+ }
+ }
+
+
+ BOOST_AUTO_TEST_CASE(se2xVkalmanFilterTest)
+ {
+ srand(time(NULL));
+
+ std::vector<SystemModelT::StateT> states;
+ std::vector<SystemModelT::ObsT> observations;
+ simulate_trajectory(states);
+ simulate_observation(states, observations);
+
+ ARMARX_INFO << "Num States: " << states.size() << " Num Obs: " << observations.size();
+
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= obs_pos_noise_std * obs_pos_noise_std;
+ R.block<1, 1>(2, 2) *= obs_rot_noise_std * obs_rot_noise_std;
+
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
+ P0.block<1, 1>(2, 2) *= initial_offset_angle * initial_offset_angle;
+ P0.block<3, 3>(3, 3) *= initial_offet_vel.norm() * initial_offet_vel.norm();
+
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha;
+ alpha(0) = 0.001; // state
+ alpha(1) = 0.001; // control
+ alpha(2) = 0.001; // update
+
+ SystemModelT::StateT state0;
+ const Vector startPos = states.at(0).pose.translation() + initial_offet_pos;
+ state0.pose = manif::SE2<T>{
+ startPos.x(), startPos.y(), states.at(0).pose.angle() + initial_offset_angle};
+ state0.velocity = states.at(0).velocity + initial_offet_vel;
+
+ UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
+
+ std::vector<SystemModelT::StateT> ukf_states;
+ std::vector<UnscentedKalmanFilter<SystemModelT>::StateCovT> ukf_Ps;
+ ukf_states.push_back(state0);
+ ukf_Ps.push_back(P0);
+
+ 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> vel_x_true, vel_y_true, vel_a_true;
+ std::vector<T> vel_x_ukf_full, vel_y_ukf_full, vel_a_ukf_full;
+ 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++)
+ {
+ // propagate
+ TIMING_START(LOOP);
+ TIMING_START(PROPAGATION);
+ ukf.propagation(SystemModelT::ControlT{}, dt);
+ TIMING_END(PROPAGATION);
+ if ((i - 1) % 5 == 0)
+ {
+ TIMING_START(UPDATE);
+ ukf.update(observations.at(i));
+ TIMING_END(UPDATE);
+ TIMING_START(REST);
+ const SystemModelT::StateT& current_state = ukf.getCurrentState();
+
+ BOOST_TEST_MESSAGE("Max Cov "
+ << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+ BOOST_TEST_MESSAGE("Diag: " << (ukf.getCurrentStateCovariance() -
+ Eigen::Matrix<T, 3, 3>::Identity())
+ .norm());
+
+ x_obs.push_back(observations.at(i)(0));
+ y_obs.push_back(observations.at(i)(1));
+ a_obs.push_back(observations.at(i)(2));
+
+ x_ukf.push_back(ukf.getCurrentState().pose.x());
+ y_ukf.push_back(ukf.getCurrentState().pose.y());
+ a_ukf.push_back(ukf.getCurrentState().pose.angle());
+
+ TIMING_END(REST);
+ }
+
+ x_true.push_back(states.at(i).pose.x());
+ y_true.push_back(states.at(i).pose.y());
+ a_true.push_back(states.at(i).pose.angle());
+
+ vel_x_true.push_back(states.at(i).velocity.x());
+ vel_y_true.push_back(states.at(i).velocity.y());
+ vel_a_true.push_back(states.at(i).velocity.angle());
+
+ x_ukf_full.push_back(ukf.getCurrentState().pose.x());
+ y_ukf_full.push_back(ukf.getCurrentState().pose.y());
+ a_ukf_full.push_back(ukf.getCurrentState().pose.angle());
+
+ vel_x_ukf_full.push_back(ukf.getCurrentState().velocity.x());
+ vel_y_ukf_full.push_back(ukf.getCurrentState().velocity.y());
+ vel_a_ukf_full.push_back(ukf.getCurrentState().velocity.angle());
+
+ ukf_states.push_back(ukf.getCurrentState());
+ ukf_Ps.push_back(ukf.getCurrentStateCovariance());
+
+ ukf_pos_var.push_back(
+ ukf.getCurrentStateCovariance().block<2, 2>(0, 0).cwiseAbs().maxCoeff());
+ ukf_rot_var.push_back(
+ ukf.getCurrentStateCovariance().block<1, 1>(2, 2).cwiseAbs().maxCoeff());
+ ukf_var.push_back(ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff());
+
+ TIMING_END(LOOP);
+ }
+
+ sciplot::Plot2D pos_plot;
+ pos_plot.xlabel("x");
+ pos_plot.ylabel("y");
+ pos_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ pos_plot.xrange(-RAD * 1.1, RAD * 1.1).yrange(-RAD * 1.1, RAD * 1.1);
+
+ pos_plot.drawCurve(x_true, y_true).label("True");
+ pos_plot.drawCurve(x_obs, y_obs).label("Obs");
+ pos_plot.drawCurveWithPoints(x_ukf, y_ukf).label("UKF");
+ pos_plot.drawCurve(x_ukf_full, y_ukf_full).label("Full UKF");
+
+
+ sciplot::Plot2D orientation_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("rot");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange(-4, 4);
+
+ sciplot::Vec x = sciplot::linspace(0.0, 1.0, a_obs.size());
+ sciplot::Vec x_t = sciplot::linspace(0.0, 1.0, a_true.size());
+ orientation_plot.drawCurve(x_t, a_true).label("True").lineWidth(1);
+ orientation_plot.drawCurve(x, a_obs).label("Obs").lineWidth(1);
+ 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 velocity_plot;
+ orientation_plot.xlabel("t");
+ orientation_plot.ylabel("vel");
+ orientation_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ orientation_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ orientation_plot.drawCurve(x_t, vel_x_true).label("x True").lineWidth(1);
+ orientation_plot.drawCurve(x_t, vel_y_true).label("y True").lineWidth(1);
+ orientation_plot.drawCurve(x_t, vel_a_true).label("a True").lineWidth(1);
+ orientation_plot.drawCurve(x_t, vel_x_ukf_full).label("x Ukf").lineWidth(1);
+ orientation_plot.drawCurve(x_t, vel_y_ukf_full).label("y Ukf").lineWidth(1);
+ orientation_plot.drawCurve(x_t, vel_a_ukf_full).label("a Ukf").lineWidth(1);
+
+
+ sciplot::Plot2D error_plot;
+ error_plot.xlabel("t");
+ error_plot.ylabel("err");
+ error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ error_plot.xrange(-0.1, 1.1).yrange("*", "*");
+
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").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}, {velocity_plot, error_plot}};
+ // Create canvas to hold figure
+ sciplot::Canvas canvas = {{fig}};
+ canvas.size(1000, 1000);
+
+ // Show the plot in a pop-up window
+ //canvas.show();
+
+ // Save the plot to a PDF file
+ canvas.save("kalman_output.pdf");
+ }
+
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/human/test/kalmanFilterTest.cpp b/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp
similarity index 98%
rename from source/armarx/navigation/human/test/kalmanFilterTest.cpp
rename to source/armarx/navigation/human/test/so2kalmanFilterTest.cpp
index 9a80c9ec..d6778e2e 100644
--- a/source/armarx/navigation/human/test/kalmanFilterTest.cpp
+++ b/source/armarx/navigation/human/test/so2kalmanFilterTest.cpp
@@ -42,7 +42,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
using Vector = Eigen::Matrix<T, 2, 1>;
using SystemModelT = kalman_filter::SystemModelSO2xR2<T>;
- constexpr T RAD = 8000;
+ constexpr T RAD = 40;
constexpr long num_timesteps = 3000;
constexpr T max_time = 1;
constexpr T dt = max_time / num_timesteps;
@@ -134,7 +134,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
- BOOST_AUTO_TEST_CASE(kalmanFilterTest)
+ BOOST_AUTO_TEST_CASE(so2KalmanFilterTest)
{
srand(time(NULL));
@@ -266,13 +266,14 @@ 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("*", "*");
-
- error_plot.drawCurve(x_t, ukf_pos_var).label("Pos cariance").lineWidth(1);
+ error_plot.drawCurve(x_t, ukf_pos_var).label("Pos variance").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);
@@ -284,7 +285,7 @@ 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");
--
GitLab