Skip to content
Snippets Groups Projects
Commit cbc24d2e authored by Tobias Gröger's avatar Tobias Gröger
Browse files

Fix kalman tests

parent b30861a0
No related branches found
Tags demo/2022-07-14/armar6a-1
3 merge requests!68Add human tracking,!53Draft: Implement basic version of kalman filter for human tracking,!28Draft: Dev -> Main
Hide whitespace changes
Inline Side-by-side
source/armarx/navigation/human/HumanSystemModel.h
+ 1
− 1
View file @ cbc24d2e
......@@ -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;
......
source/armarx/navigation/human/UnscentedKalmanFilter_impl.cpp
+ 2
− 0
View file @ cbc24d2e
......@@ -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>>;
source/armarx/navigation/human/test/se2KalmanFilterTest.cpp
+ 17
− 16
View file @ cbc24d2e
......@@ -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("*", "*");
......
source/armarx/navigation/human/test/se2xVkalmanFilterTest.cpp
+ 14
− 17
View file @ cbc24d2e
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment