diff --git a/source/armarx/navigation/human/HumanSystemModel.h b/source/armarx/navigation/human/HumanSystemModel.h
index 8cbda52539bc4bbeb935e052ba008fdd2018c6b0..036ed53718f9eda5495bb7fa076b642f17ffc4a5 100644
--- a/source/armarx/navigation/human/HumanSystemModel.h
+++ b/source/armarx/navigation/human/HumanSystemModel.h
@@ -142,7 +142,7 @@ namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
static_assert(std::is_floating_point_v<floatT>);
struct dim
{
- static constexpr long state = 6, control = 0, obs = 3;
+ static constexpr long state = 6, control = 1, obs = 3;
};
using FloatT = floatT;
diff --git a/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
index 168a9e036b3913cf5c548f4a62c680cd0ae06abe..37360a4b44fdd46fa1b69580fbdaef14c7f82846 100644
--- a/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
+++ b/source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
@@ -10,3 +10,5 @@ template class UnscentedKalmanFilter<armarx::navigation::components::dynamic_sce
template class UnscentedKalmanFilter<
armarx::navigation::components::dynamic_scene_provider::kalman_filter::SystemModelSE2<double>>;
+template class UnscentedKalmanFilter<armarx::navigation::components::dynamic_scene_provider::
+ kalman_filter::SystemModelSE2xV<double>>;
diff --git a/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
index 61d375304fec84386bb946890c0838a9dc3ebade..3746662dc0a006452beedd7c48237d1f20c25c7d 100644
--- a/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
+++ b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
@@ -42,18 +42,18 @@ namespace armarx::navigation::components::dynamic_scene_provider
using Vector = Eigen::Matrix<T, 2, 1>;
using SystemModelT = kalman_filter::SystemModelSE2<T>;
- constexpr T RAD = 1;
+ constexpr T RAD = 10;
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);
+ constexpr T rot_noise_std = 0.2;
+ constexpr T pos_noise_std = 2;
+ constexpr T obs_rot_noise_std = 0.2;
+ constexpr T obs_pos_noise_std = 2;
+ constexpr T initial_offset_angle = 0.1 * 10 * M_PI / 180;
+ const Vector initial_offet_pos = 0.1 * RAD * Vector(1, 0.5);
void
@@ -78,7 +78,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
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};
+ Eigen::Vector2d euclidean_velocity = RAD * Eigen::Vector2d{0, c};
SystemModelT::ControlT control;
//control.velocity.coeffs() << (pos - last_pos) / dt, c;
control.velocity.coeffs() << euclidean_velocity, c;
@@ -211,9 +211,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
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));
+ manif::SE2Tangent<T> obs{observations.at(i)};
+ x_obs.push_back(obs.exp().x());
+ y_obs.push_back(obs.exp().y());
+ a_obs.push_back(obs.exp().angle());
x_ukf.push_back(ukf.getCurrentState().pose.x());
y_ukf.push_back(ukf.getCurrentState().pose.y());
@@ -245,7 +246,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
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(-RAD * 1.2, RAD * 1.2).yrange(-RAD * 1.2, RAD * 1.2);
//pos_plot.xrange(-4, 4).yrange(-4, 4);
pos_plot.drawCurve(x_true, y_true).label("True");
@@ -255,8 +256,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
sciplot::Plot2D orientation_plot;
- orientation_plot.xlabel("x");
- orientation_plot.ylabel("y");
+ 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);
@@ -269,8 +270,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
sciplot::Plot2D error_plot;
- error_plot.xlabel("x");
- error_plot.ylabel("y");
+ error_plot.xlabel("t");
+ error_plot.ylabel("err");
error_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
error_plot.xrange(-0.1, 1.1).yrange("*", "*");
diff --git a/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
index 5187e87ac602b797de29b783f55a8ce1c0304aeb..fa182266ecb0edb84b6cbfdd79b415cfdad97eb4 100644
--- a/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
+++ b/source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
@@ -52,7 +52,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
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);
+ const Eigen::Matrix<T, 3, 1> initial_offet_vel =
+ 0.8 * RAD * Eigen::Matrix<T, 3, 1>(1, 0.5, 0.5);
void
@@ -181,13 +182,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
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));
@@ -255,17 +252,17 @@ namespace armarx::navigation::components::dynamic_scene_provider
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("*", "*");
+ velocity_plot.xlabel("t");
+ velocity_plot.ylabel("vel");
+ velocity_plot.legend().atOutsideBottom().displayHorizontal().displayExpandWidthBy(2);
+ velocity_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);
+ velocity_plot.drawCurve(x_t, vel_x_true).label("x True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_y_true).label("y True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_a_true).label("a True").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_x_ukf_full).label("x Ukf").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_y_ukf_full).label("y Ukf").lineWidth(1);
+ velocity_plot.drawCurve(x_t, vel_a_ukf_full).label("a Ukf").lineWidth(1);
sciplot::Plot2D error_plot;
@@ -286,10 +283,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("se2xV_kalman_output.pdf");
}
} // namespace armarx::navigation::components::dynamic_scene_provider