Add comments to HumanFilter.cpp

source/armarx/navigation/human/HumanFilter.cpp

@@ -3,6 +3,12 @@
 namespace armarx::navigation::human
 {
 
+    /**
+     * @brief HumanFilter::HumanFilter creates a new filter that filters 2D poses (position and
+     * rotation in a two dimensional plane)
+     * @param pose0 the first known pose of the human
+     * @param detectionTime the point of detection
+     */
     HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
     {
         //initialize new human for detected human
@@ -12,7 +18,6 @@ namespace armarx::navigation::human
 
 
         //initialize new kalman filter for new detected human
-
         UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
             UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
         Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
@@ -37,6 +42,12 @@ namespace armarx::navigation::human
         ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
     }
 
+    /**
+     * @brief HumanFilter::propagation propagate the system model to the given point in time. This
+     * means that the human pose is updated according to the filters prediction for the given point
+     * in time
+     * @param detectionTime the point in time for which the pose should be predicted
+     */
     void
     HumanFilter::propagation(const armarx::core::time::DateTime& detectionTime)
     {
@@ -49,11 +60,18 @@ namespace armarx::navigation::human
         human.pose = fromUkfState(ukf->getCurrentState());
     }
 
+    /**
+     * @brief HumanFilter::update update the filter a new detected pose of the tracked human and
+     * the detection time
+     * @param pose the new detected pose of the human
+     * @param detectionTime the detection time
+     */
     void
     HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
     {
         double dt = (detectionTime - human.detectionTime).toSecondsDouble();
 
+        //update ukf with new observation
         SystemModelT::ObsT observation = toUkfObs(pose);
         ukf->update(observation);
 
@@ -62,14 +80,22 @@ namespace armarx::navigation::human
         // calculate velocity
         Eigen::Vector2f ds = newPose.translation() - oldPose.translation();
         Eigen::Vector2f linVelocity = ds / dt;
+        // apply exponential smoothing to velocity
+        //TODO try other approaches?
         Eigen::Vector2f newVelocity =
             params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
 
+        //update stored information about the human
         human.detectionTime = detectionTime;
         human.pose = newPose;
         human.linearVelocity = newVelocity;
     }
 
+    /**
+     * @brief HumanFilter::get returns the human whose pose is filtered containing the most recent
+     * state
+     * @return the human
+     */
     const Human&
     HumanFilter::get() const
     {