diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt index 4f772ebf5a6c84ae5525edd3eba9e6d681f368a9..14deaae1a421feda7d89b83a65b6b64a676a16dd 100644 --- a/source/armarx/navigation/human/CMakeLists.txt +++ b/source/armarx/navigation/human/CMakeLists.txt @@ -21,6 +21,7 @@ armarx_add_library(teb_human HumanSystemModel.cpp UnscentedKalmanFilter_impl.cpp HumanTracker.cpp + HumanFilter.cpp HEADERS types.h aron_conversions.h @@ -28,6 +29,7 @@ armarx_add_library(teb_human ProxemicZoneCreator.h HumanSystemModel.h HumanTracker.h + HumanFilter.h ) find_package(sciplot) diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..436a9cebd18cdb70d3100edcdc80625de747a6c4 --- /dev/null +++ b/source/armarx/navigation/human/HumanFilter.cpp @@ -0,0 +1,10 @@ +#include "HumanFilter.h" + +namespace armarx::navigation::human +{ + + HumanFilter::HumanFilter() + { + } + +} // namespace armarx::navigation::human diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h new file mode 100644 index 0000000000000000000000000000000000000000..9f4cf3ec4d6c06abb07a80c59d901cfe2a4c4272 --- /dev/null +++ b/source/armarx/navigation/human/HumanFilter.h @@ -0,0 +1,34 @@ +/** + * 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 ) + * @author Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu ) + * @date 2022 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +namespace armarx::navigation::human +{ + + class HumanFilter + { + public: + HumanFilter(); + }; + +} // namespace armarx::navigation::human diff --git a/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp index 011d96b8ae163e940f431a4db11a3c14827aa633..c83f4db5b4fedc49a6af0077229a075ea9e7cd25 100644 --- a/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp +++ b/source/armarx/navigation/human/test/se2KalmanFilterTest.cpp @@ -48,10 +48,10 @@ namespace armarx::navigation::human 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.2; - constexpr T pos_noise_std = 2; - constexpr T obs_rot_noise_std = 0.2; - constexpr T obs_pos_noise_std = 2; + constexpr T rot_noise_std = 0.1; + constexpr T pos_noise_std = 1; + constexpr T obs_rot_noise_std = 0.1; + constexpr T obs_pos_noise_std = 1; constexpr T initial_offset_angle = 0.1 * 10 * M_PI / 180; const Vector initial_offet_pos = 0.1 * RAD * Vector(1, 0.5); @@ -198,7 +198,7 @@ namespace armarx::navigation::human TIMING_START(PROPAGATION); ukf.propagation(omegas.at(i - 1), dt); TIMING_END(PROPAGATION); - if ((i - 1) % 50 == 0) + if ((i - 1) % 1 == 0) { TIMING_START(UPDATE); ukf.update(observations.at(i));