Implement usage of ukf in update step

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

@@ -212,6 +212,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
         trackedHuman->associated = true;
         detectedHuman->associated = true;
 
+        //estimate velocity via exponential smoothing
+
         float dt =
             (detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
         Eigen::Vector2f ds =
@@ -222,18 +224,38 @@ namespace armarx::navigation::components::dynamic_scene_provider
             parameters.velocityAlpha * linVelocity +
             (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
 
-        //TODO initialize with velocity (control variables) from tracked human
-        UnscentedKalmanFilter<SystemModelT>::ControlT omega;
-        trackedHuman->ukf.propagation(omega, dt);
+        float angle_det = Eigen::Rotation2Df(detectedHuman->pose.linear()).angle();
+        float angle_track = Eigen::Rotation2Df(trackedHuman->human.pose.linear()).angle();
+        //TODO Check angular velocity! Calculation at turning point may be wrong (jump from +pi to -pi)
+        float angular_velocity = (angle_det - angle_track) / dt;
+
+
+        //estimate pose via ukf
 
-        //TODO initialize with detected human data
+        //initialize with estimated velocity (control variables) from tracked human
+        UnscentedKalmanFilter<SystemModelT>::ControlT control;
+        control.angular_velocity.coeffs() << static_cast<T>(angular_velocity);
+        control.euclidean_velocity << velocity.cast<T>();
+
+        //propagate control input for time diff
+        trackedHuman->ukf.propagation(control, dt);
+
+        //initialize observation with detected human data
         UnscentedKalmanFilter<SystemModelT>::ObsT observation;
+        observation.segment(0, 1) =
+            Eigen::Matrix<T, 1, 1>{Eigen::Rotation2Df(detectedHuman->pose.linear()).angle()};
+        observation.segment(1, 2) = detectedHuman->pose.translation().cast<T>();
+
+        //update with new observation
         trackedHuman->ukf.update(observation);
 
-        //TODO use state as new pose of tracked human
+        //use current state as new pose of tracked human
         UnscentedKalmanFilter<SystemModelT>::StateT state = trackedHuman->ukf.getCurrentState();
+        core::Pose2D filteredPose = core::Pose2D::Identity();
+        filteredPose.translation() = state.position.cast<float>();
+        filteredPose.linear() = Eigen::Rotation2Df(state.orientation.angle()).toRotationMatrix();
 
-        trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
+        trackedHuman->human = {filteredPose, velocity, detectedHuman->detectionTime};
         trackedHuman->trackingId = detectedHuman->trackingId;
     }