Draft: Implement basic version of kalman filter for human tracking

Implement ukfm from armarx/RobotAPI/libraries

source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp

@@ -11,12 +11,12 @@
 namespace armarx::navigation::components::dynamic_scene_provider
 {
 
-    //TODO which values are here good?
-    constexpr T acc_noise_std = 0.01;
-    constexpr T om_noise_std = 0.01;
+    //TODO which values are appropriate?
+    constexpr T pos_noise_std = 0.01;
+    constexpr T rot_noise_std = 0.01;
     constexpr T obs_noise_std = 0.01;
-    constexpr T initial_offset_angle = 0.0 * 10 * M_PI / 180;
-    const Vector initial_offset_pos = 0.5 * Vector(1, 0.7);
+    constexpr T initial_pos_state_cov = 0.01;
+    constexpr T initial_rot_state_cov = 0.01;
 
     HumanTracker::DetectedHuman
     convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
@@ -82,28 +82,34 @@ namespace armarx::navigation::components::dynamic_scene_provider
                                    .linearVelocity = Eigen::Vector2f::Zero(),
                                    .detectionTime = detectedHuman.detectionTime};
 
-
                 //initialize new kalman filter for new detected human
+
                 UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
                     UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
-                Q.block<2, 2>(0, 0) *= acc_noise_std * acc_noise_std;
-                Q.block<1, 1>(2, 2) *= om_noise_std * om_noise_std;
+                //rotation noise at (0,0)
+                Q.block<1, 1>(0, 0) *= rot_noise_std * rot_noise_std;
+                //position noise at (1,1) and (2,2)
+                Q.block<2, 2>(1, 1) *= pos_noise_std * pos_noise_std;
 
+                //observation noise at (0,0), (1,1) and (2,2)
                 UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
                     UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std *
                     obs_noise_std;
 
                 UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
                     UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
-                P0.block<1, 1>(2, 2) *= initial_offset_angle * initial_offset_angle;
-                P0.block<2, 2>(0, 0) *= initial_offset_pos.norm() * initial_offset_pos.norm();
+                P0.block<1, 1>(0, 0) *= initial_rot_state_cov * initial_rot_state_cov;
+                P0.block<2, 2>(1, 1) *= initial_pos_state_cov * initial_pos_state_cov;
 
                 UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
                     UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
 
+                //initial position and orientation according to detected human
                 SystemModelT::StateT state0;
-                state0.position = initial_offset_pos;
-                state0.orientation = manif::SO2<T>{initial_offset_angle};
+                //initialize with detected human pose position
+                state0.position = human.pose.translation();
+                //initialize with SO2 matrix according to detected human pose rotation
+                state0.orientation = manif::SO2<T>{Eigen::Rotation2Df(human.pose.linear()).angle()};
 
                 UnscentedKalmanFilter<SystemModelT> ukf{Q, R, alpha, state0, P0};
 
@@ -216,6 +222,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
             parameters.velocityAlpha * linVelocity +
             (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
 
+        //TODO extract new pose of tracked human from ukf after feeding the detected human
+
         trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
         trackedHuman->trackingId = detectedHuman->trackingId;
     }