fix(multi_object_tracker): tune parameters for pedestrian tracker

perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp

@@ -173,7 +173,7 @@
 
 bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
 {
-  /*  static bicycle model (constant slip angle, constant velocity)
+  /*  Motion model: static bicycle model (constant slip angle, constant velocity)
    *
    * w_k = vel_k * sin(slip_k) / l_r
    * x_{k+1}   = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt
@@ -291,12 +291,6 @@
   return true;
 }
 
-/**
- * @brief measurement update with ekf on receiving detected_object
- *
- * @param object : Detected object
- * @return bool : measurement is updatable
- */
 bool BicycleTracker::measureWithPose(
   const autoware_auto_perception_msgs::msg::DetectedObject & object)
 {
@@ -333,19 +327,16 @@
   const int dim_y =
     use_orientation_information ? 3 : 2;  // pos x, pos y, (pos yaw) depending on Pose output
 
-  /* Set measurement matrix */
+  // Set measurement matrix C and observation vector Y
   Eigen::MatrixXd Y(dim_y, 1);
+  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
   Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x;
   Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y;
-
-  /* Set measurement matrix */
-  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
   C(0, IDX::X) = 1.0;  // for pos x
   C(1, IDX::Y) = 1.0;  // for pos y
 
-  /* Set measurement noise covariance */
+  // Set noise covariance matrix R
   Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
-
   if (!object.kinematics.has_position_covariance) {
     R(0, 0) = ekf_params_.r_cov_x;  // x - x
     R(0, 1) = 0.0;                  // x - y
@@ -471,6 +462,7 @@
   tmp_ekf_for_no_update.getX(X_t);
   tmp_ekf_for_no_update.getP(P);
 
+  /*  put predicted pose and twist to output object  */
   auto & pose_with_cov = object.kinematics.pose_with_covariance;
   auto & twist_with_cov = object.kinematics.twist_with_covariance;
 

perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

@@ -188,7 +188,7 @@
 
 bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
 {
-  /*  static bicycle model (constant slip angle, constant velocity)
+  /*  Motion model: static bicycle model (constant slip angle, constant velocity)
    *
    * w_k = vel_k * sin(slip_k) / l_r
    * x_{k+1}   = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt
@@ -362,19 +362,18 @@
     last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_,
     bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_);
 
-  /* Set measurement matrix and noise covariance*/
+  // Set measurement matrix C and observation vector Y
   Eigen::MatrixXd Y(dim_y, 1);
   Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
-  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
-
   Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x;
   Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y;
   Y(IDX::YAW, 0) = measurement_yaw;
   C(0, IDX::X) = 1.0;    // for pos x
   C(1, IDX::Y) = 1.0;    // for pos y
   C(2, IDX::YAW) = 1.0;  // for yaw
 
-  /* Set measurement noise covariance */
+  // Set noise covariance matrix R
+  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
   if (!object.kinematics.has_position_covariance) {
     const double cos_yaw = std::cos(measurement_yaw);
     const double sin_yaw = std::sin(measurement_yaw);
@@ -459,6 +458,7 @@
   // update lf, lr
   lf_ = std::max(bounding_box_.length * 0.3, 1.5);   // 30% front from the center, minimum of 1.5m
   lr_ = std::max(bounding_box_.length * 0.25, 1.5);  // 25% rear from the center, minimum of 1.5m
+
   return true;
 }
 
@@ -597,6 +597,7 @@
   const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
   object.shape.footprint =
     tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
+
   return true;
 }
 

perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

@@ -188,7 +188,7 @@
 
 bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const
 {
-  /*  static bicycle model (constant slip angle, constant velocity)
+  /*  Motion model: static bicycle model (constant slip angle, constant velocity)
    *
    * w_k = vel_k * sin(slip_k) / l_r
    * x_{k+1}   = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt
@@ -362,19 +362,18 @@
     last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_,
     bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_);
 
-  /* Set measurement matrix and noise covariance*/
+  // Set measurement matrix C and observation vector Y
   Eigen::MatrixXd Y(dim_y, 1);
   Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
-  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
-
   Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x;
   Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y;
   Y(IDX::YAW, 0) = measurement_yaw;
   C(0, IDX::X) = 1.0;    // for pos x
   C(1, IDX::Y) = 1.0;    // for pos y
   C(2, IDX::YAW) = 1.0;  // for yaw
 
-  /* Set measurement noise covariance */
+  // Set noise covariance matrix R
+  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
   if (!object.kinematics.has_position_covariance) {
     const double cos_yaw = std::cos(measurement_yaw);
     const double sin_yaw = std::sin(measurement_yaw);
@@ -598,6 +597,7 @@
   const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
   object.shape.footprint =
     tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
+
   return true;
 }
 

perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp

@@ -19,7 +19,6 @@
 #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp"
 
 #include "multi_object_tracker/utils/utils.hpp"
-#include "object_recognition_utils/object_recognition_utils.hpp"
 
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <tier4_autoware_utils/math/normalization.hpp>
@@ -35,6 +34,7 @@
 #else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
+#include "object_recognition_utils/object_recognition_utils.hpp"
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
@@ -52,37 +52,41 @@
 {
   object_ = object;
 
-  // initialize params
-  float q_stddev_x = 0.4;                                     // [m/s]
-  float q_stddev_y = 0.4;                                     // [m/s]
-  float q_stddev_yaw = tier4_autoware_utils::deg2rad(20);     // [rad/s]
-  float q_stddev_vx = tier4_autoware_utils::kmph2mps(5);      // [m/(s*s)]
-  float q_stddev_wz = tier4_autoware_utils::deg2rad(20);      // [rad/(s*s)]
-  float r_stddev_x = 0.4;                                     // [m]
-  float r_stddev_y = 0.4;                                     // [m]
-  float r_stddev_yaw = tier4_autoware_utils::deg2rad(30);     // [rad]
-  float p0_stddev_x = 1.0;                                    // [m/s]
-  float p0_stddev_y = 1.0;                                    // [m/s]
-  float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000);  // [rad/s]
-  float p0_stddev_vx = tier4_autoware_utils::kmph2mps(5);     // [m/(s*s)]
-  float p0_stddev_wz = tier4_autoware_utils::deg2rad(10);     // [rad/(s*s)]
-  ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
-  ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
-  ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
-  ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
-  ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
+  // Initialize parameters
+  // measurement noise covariance
+  float r_stddev_x = 0.4;                                  // [m]
+  float r_stddev_y = 0.4;                                  // [m]
+  float r_stddev_yaw = tier4_autoware_utils::deg2rad(30);  // [rad]
   ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
   ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
   ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
+  // initial state covariance
+  float p0_stddev_x = 2.0;                                    // [m]
+  float p0_stddev_y = 2.0;                                    // [m]
+  float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000);  // [rad]
+  float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120);   // [m/s]
+  float p0_stddev_wz = tier4_autoware_utils::deg2rad(360);    // [rad/s]
   ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
   ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
   ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
   ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
   ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0);
+  // process noise covariance
+  float q_stddev_x = 0.4;                                  // [m/s]
+  float q_stddev_y = 0.4;                                  // [m/s]
+  float q_stddev_yaw = tier4_autoware_utils::deg2rad(20);  // [rad/s]
+  float q_stddev_vx = tier4_autoware_utils::kmph2mps(5);   // [m/(s*s)]
+  float q_stddev_wz = tier4_autoware_utils::deg2rad(30);   // [rad/(s*s)]
+  ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
+  ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
+  ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
+  ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
+  ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
+  // limitations
   max_vx_ = tier4_autoware_utils::kmph2mps(10);  // [m/s]
   max_wz_ = tier4_autoware_utils::deg2rad(30);   // [rad/s]
 
-  // initialize X matrix
+  // initialize state vector X
   Eigen::MatrixXd X(ekf_params_.dim_x, 1);
   X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
   X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
@@ -95,7 +99,7 @@
     X(IDX::WZ) = 0.0;
   }
 
-  // initialize P matrix
+  // initialize state covariance matrix P
   Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
   if (!object.kinematics.has_position_covariance) {
     const double cos_yaw = std::cos(X(IDX::YAW));
@@ -154,7 +158,7 @@
 
 bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
 {
-  /*  == Nonlinear model ==
+  /*  Motion model: Constant turn-rate motion model (CTRV)
    *
    * x_{k+1}   = x_k + vx_k * cos(yaw_k) * dt
    * y_{k+1}   = y_k + vx_k * sin(yaw_k) * dt
@@ -164,7 +168,7 @@
    *
    */
 
-  /*  == Linearized model ==
+  /*  Jacobian Matrix
    *
    * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt,  0]
    *     [ 0, 1,  vx*cos(yaw)*dt, sin(yaw)*dt,  0]
@@ -173,30 +177,30 @@
    *     [ 0, 0,               0,           0,  1]
    */
 
-  // X t
+  // Current state vector X t
   Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);  // predicted state
   ekf.getX(X_t);
   const double cos_yaw = std::cos(X_t(IDX::YAW));
   const double sin_yaw = std::sin(X_t(IDX::YAW));
   const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));
 
-  // X t+1
+  // Predict state vector X t+1
   Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1);                // predicted state
   X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt;  // dx = v * cos(yaw)
   X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt;  // dy = v * sin(yaw)
   X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt;      // dyaw = omega
   X_next_t(IDX::VX) = X_t(IDX::VX);
   X_next_t(IDX::WZ) = X_t(IDX::WZ);
 
-  // A
+  // State transition matrix A
   Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
   A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt;
   A(IDX::X, IDX::VX) = cos_yaw * dt;
   A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt;
   A(IDX::Y, IDX::VX) = sin_yaw * dt;
   A(IDX::YAW, IDX::WZ) = dt;
 
-  // Q
+  // Process noise covariance Q
   Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
   // Rotate the covariance matrix according to the vehicle yaw
   // because q_cov_x and y are in the vehicle coordinate system.
@@ -240,18 +244,16 @@
   //   if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false;
   // }
 
-  /* Set measurement matrix */
+  // Set measurement matrix C and observation vector Y
   Eigen::MatrixXd Y(dim_y, 1);
+  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
   Y << object.kinematics.pose_with_covariance.pose.position.x,
     object.kinematics.pose_with_covariance.pose.position.y;
-
-  /* Set measurement matrix */
-  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
   C(0, IDX::X) = 1.0;  // for pos x
   C(1, IDX::Y) = 1.0;  // for pos y
   // C(2, IDX::YAW) = 1.0;  // for yaw
 
-  /* Set measurement noise covariance */
+  // Set noise covariance matrix R
   Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
   if (!object.kinematics.has_position_covariance) {
     R(0, 0) = ekf_params_.r_cov_x;  // x - x
@@ -270,6 +272,8 @@
     // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y];
     // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
   }
+
+  // ekf update
   if (!ekf_.update(Y, C, R)) {
     RCLCPP_WARN(logger_, "Cannot update");
   }
@@ -345,7 +349,7 @@
   object.object_id = getUUID();
   object.classification = getClassification();
 
-  // predict kinematics
+  // predict state
   KalmanFilter tmp_ekf_for_no_update = ekf_;
   const double dt = (time - last_update_time_).seconds();
   if (0.001 /*1msec*/ < dt) {
@@ -356,6 +360,7 @@
   tmp_ekf_for_no_update.getX(X_t);
   tmp_ekf_for_no_update.getP(P);
 
+  /*  put predicted pose and twist to output object  */
   auto & pose_with_cov = object.kinematics.pose_with_covariance;
   auto & twist_with_cov = object.kinematics.twist_with_covariance;
 
@@ -399,6 +404,7 @@
   constexpr double vz_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
   constexpr double wx_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
   constexpr double wy_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
+
   twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
   twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
   twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;