Draft: Implement basic version of kalman filter for human tracking

Implement ukfm from armarx/RobotAPI/libraries

source/armarx/navigation/human/test/kalmanFilterTest.cpp

@@ -50,8 +50,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
     constexpr T rot_noise_std = 0.01;
     constexpr T pos_noise_std = 0.01;
-    constexpr T obs_noise_std = 0.1;
-    constexpr T initial_offset_angle = 0.0 * 10 * M_PI / 180;
+    constexpr T obs_noise_std = 0.01;
+    constexpr T initial_offset_angle = 0.1 * 10 * M_PI / 180;
     const Vector initial_offet_pos = 0.1 * Vector(1, 0.5);
 
 
@@ -59,9 +59,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
     simulate_trajectory(std::vector<SystemModelT::StateT>& states,
                         std::vector<SystemModelT::ControlT>& omegas)
     {
+        constexpr T startAngle = 0;
+
         SystemModelT::StateT state;
         state.position = Vector{1, 0};
-        state.orientation = manif::SO2<T>(0);
+        state.orientation = manif::SO2<T>(startAngle);
         states.push_back(state);
 
         for (int i = 1; i < num_timesteps; i++)
@@ -86,12 +88,17 @@ namespace armarx::navigation::components::dynamic_scene_provider
             const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
                 state, control, SystemModelT::ControlNoiseT::Zero(), dt);
 
-            T pos_diff1 = (propagated.position - pos).norm();
-            BOOST_TEST(pos_diff1 < 2e-5,
-                       "propagated position differs too much from real position: " << pos_diff1);
-
             state.position = pos;
-            state.orientation = manif::SO2<T>(angle);
+            state.orientation = manif::SO2<T>(angle + startAngle);
+
+            T pos_diff = (propagated.position - state.position).norm();
+            BOOST_TEST(pos_diff < 2e-5,
+                       "propagated position differs too much from real position: " << pos_diff);
+
+            T rot_diff = propagated.orientation.angle() - state.orientation.angle();
+            BOOST_TEST(rot_diff < 2e-5,
+                       "propagated rotation differs too much from real orientation: " << rot_diff);
+
             states.push_back(propagated);
 
 
@@ -116,6 +123,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
             BOOST_TEST(
                 (obs - true_obs).norm() < std::numeric_limits<T>::epsilon(),
                 "observation differs too much from real observation: " << (obs - true_obs).norm());
+
             observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
         }
     }
@@ -151,7 +159,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
             UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
 
         SystemModelT::StateT state0;
-        state0.orientation = manif::SO2<T>(initial_offset_angle);
+        state0.orientation = manif::SO2<T>(states.at(0).orientation.angle() + initial_offset_angle);
         state0.position = states.at(0).position + initial_offet_pos;
 
         UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
@@ -162,8 +170,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
         ukf_Ps.push_back(P0);
 
         std::vector<T> x_true, y_true, x_obs, y_obs, x_ukf, y_ukf;
-        std::vector<T> vx_true, vy_true, vx_ukf, vy_ukf;
         std::vector<T> a_true, a_obs, a_ukf;
+        std::vector<T> x_ukf_full, y_ukf_full, a_ukf_full;
 
         for (int i = 1; i < num_timesteps; i++)
         {
@@ -172,8 +180,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
             TIMING_START(PROPAGATION);
             ukf.propagation(omegas.at(i - 1), dt);
             TIMING_END(PROPAGATION);
-            if ((i - 1) % 100 == 0)
+            if ((i - 1) % 101 == 0)
             {
+                // dirty hack
+                //if (std::abs(M_PI - std::abs(observations.at(i)(0))) < 0.1)
+                //{
+                //   continue;
+                //}
+
                 TIMING_START(UPDATE);
                 ukf.update(observations.at(i));
                 TIMING_END(UPDATE);
@@ -194,18 +208,21 @@ namespace armarx::navigation::components::dynamic_scene_provider
 
                 x_obs.push_back(observations.at(i)(1));
                 y_obs.push_back(observations.at(i)(2));
+                a_obs.push_back(observations.at(i)(0));
 
                 x_ukf.push_back(ukf.getCurrentState().position.x());
                 y_ukf.push_back(ukf.getCurrentState().position.y());
-
-                a_true.push_back(states.at(i).orientation.log().coeffs()(0));
-                a_obs.push_back(observations.at(i)(0));
                 a_ukf.push_back(ukf.getCurrentState().orientation.log().coeffs()(0));
 
                 TIMING_END(REST);
             }
             x_true.push_back(states.at(i).position.x());
             y_true.push_back(states.at(i).position.y());
+            a_true.push_back(states.at(i).orientation.log().coeffs()(0));
+
+            x_ukf_full.push_back(ukf.getCurrentState().position.x());
+            y_ukf_full.push_back(ukf.getCurrentState().position.y());
+            a_ukf_full.push_back(ukf.getCurrentState().orientation.log().coeffs()(0));
 
             ukf_states.push_back(ukf.getCurrentState());
             ukf_Ps.push_back(ukf.getCurrentStateCovariance());
@@ -218,27 +235,34 @@ 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(-1.6, 1.6).yrange(-1.6, 1.6);
+        //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.drawCurve(x_ukf, y_ukf).label("UKF");
+        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_true.size());
-        orientation_plot.drawCurve(x, a_true).label("True").lineWidth(1);
+        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);
 
 
         // Create figure to hold plot
         sciplot::Figure fig = {{pos_plot}, {orientation_plot}};
         // Create canvas to hold figure
         sciplot::Canvas canvas = {{fig}};
-        canvas.size(600, 600);
+        canvas.size(600, 1200);
 
         // Show the plot in a pop-up window
         canvas.show();