Create new class HumanFilter

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)

source/armarx/navigation/human/HumanFilter.cpp

+#include "HumanFilter.h"
+
+namespace armarx::navigation::human
+{
+
+    HumanFilter::HumanFilter()
+    {
+    }
+
+} // namespace armarx::navigation::human

source/armarx/navigation/human/HumanFilter.h

+/**
+ * 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

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));