fix(multi_object_tracking): mot bicycle model revision, tracking_object_merger bugfix

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp

@@ -32,16 +32,18 @@ class BicycleTracker : public Tracker
 private:
   KalmanFilter ekf_;
   rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
+  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
   struct EkfParams
   {
     char dim_x = 5;
-    float q_cov_x;
-    float q_cov_y;
-    float q_cov_yaw;
-    float q_cov_slip;
-    float q_cov_vx;
-    float p0_cov_vx;
+    float q_stddev_acc_long;
+    float q_stddev_acc_lat;
+    float q_stddev_yaw_rate_min;
+    float q_stddev_yaw_rate_max;
+    float q_cov_slip_rate_min;
+    float q_cov_slip_rate_max;
+    float q_max_slip_angle;
+    float p0_cov_vel;
     float p0_cov_slip;
     float r_cov_x;
     float r_cov_y;
@@ -51,7 +53,7 @@ class BicycleTracker : public Tracker
     float p0_cov_yaw;
   } ekf_params_;
 
-  double max_vx_;
+  double max_vel_;
   double max_slip_;
   double lf_;
   double lr_;

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

@@ -32,26 +32,28 @@ class BigVehicleTracker : public Tracker
 private:
   KalmanFilter ekf_;
   rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
+  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
   struct EkfParams
   {
     char dim_x = 5;
-    float q_cov_x;
-    float q_cov_y;
-    float q_cov_yaw;
-    float q_cov_slip;
-    float q_cov_vx;
-    float p0_cov_vx;
+    float q_stddev_acc_long;
+    float q_stddev_acc_lat;
+    float q_stddev_yaw_rate_min;
+    float q_stddev_yaw_rate_max;
+    float q_cov_slip_rate_min;
+    float q_cov_slip_rate_max;
+    float q_max_slip_angle;
+    float p0_cov_vel;
     float p0_cov_slip;
     float r_cov_x;
     float r_cov_y;
     float r_cov_yaw;
-    float r_cov_vx;
+    float r_cov_vel;
     float p0_cov_x;
     float p0_cov_y;
     float p0_cov_yaw;
   } ekf_params_;
-  double max_vx_;
+  double max_vel_;
   double max_slip_;
   double lf_;
   double lr_;

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp

@@ -33,28 +33,30 @@ class NormalVehicleTracker : public Tracker
 private:
   KalmanFilter ekf_;
   rclcpp::Time last_update_time_;
-  enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
+  enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 };
 
   struct EkfParams
   {
     char dim_x = 5;
-    float q_cov_x;
-    float q_cov_y;
-    float q_cov_yaw;
-    float q_cov_slip;
-    float q_cov_vx;
-    float p0_cov_vx;
+    float q_stddev_acc_long;
+    float q_stddev_acc_lat;
+    float q_stddev_yaw_rate_min;
+    float q_stddev_yaw_rate_max;
+    float q_cov_slip_rate_min;
+    float q_cov_slip_rate_max;
+    float q_max_slip_angle;
+    float p0_cov_vel;
     float p0_cov_slip;
     float r_cov_x;
     float r_cov_y;
     float r_cov_yaw;
-    float r_cov_vx;
+    float r_cov_vel;
     float p0_cov_x;
     float p0_cov_y;
     float p0_cov_yaw;
   } ekf_params_;
 
-  double max_vx_;
+  double max_vel_;
   double max_slip_;
   double lf_;
   double lr_;
@@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
   void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform);
+  double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   virtual ~NormalVehicleTracker() {}
 };
 

perception/multi_object_tracker/models.md

@@ -11,21 +11,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude.
 - state transition equation
 
 $$
-\begin{align*}
-x_{k+1}   &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\
-y_{k+1}   &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\
-\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\
-v_{x_{k+1}}  &= v_{x_k} \\
-\dot{\psi}_{k+1}  &= \dot{\psi}_k \\
-\end{align*}
+\begin{aligned}
+x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\
+y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\
+\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\
+v_{k+1} & = v_{k} \\
+\dot\psi_{k+1} & = \dot\psi_{k} \\
+\end{aligned}
 $$
 
 - jacobian
 
 $$
 A = \begin{bmatrix}
-1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
-0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
+1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
+0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
 0 & 0 & 1 & 0 & dt \\
 0 & 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 0 & 1 \\
@@ -40,17 +40,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe
 ![kinematic_bicycle_model](image/kinematic_bicycle_model.png)
 
 - **state variable**
-  - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ )
-  - $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$
+  - pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ )
+  - $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$
 - **Prediction Equation**
   - $dt$: sampling time
+  - $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity
 
 $$
 \begin{aligned}
-x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\
-y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\
-v_{k+1} & =v_{k} \\
-\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\
+x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t}
+            -\frac{1}{2}  \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
+y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t}
+            +\frac{1}{2}  \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
+\psi_{k+1} & =\psi_{k} + w_k {d t} \\
+v_{k+1} & = v_{k} \\
 \beta_{k+1} & =\beta_{k}
 \end{aligned}
 $$
@@ -59,9 +62,15 @@ $$
 
 $$
 \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc}
-1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\
-0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\
-0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\
+1 & 0
+ & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\
+0 & 1
+ & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
+ & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\
+0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\
 0 & 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 0 & 1
 \end{array}\right]

perception/multi_object_tracker/src/data_association/data_association.cpp

@@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
             measurement_object.kinematics.pose_with_covariance.pose.position,
             tracked_object.kinematics.pose_with_covariance.pose.position,
             getXYCovariance(tracked_object.kinematics.pose_with_covariance));
-          if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false;
+          if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false;
         }
         // 2d iou gate
         if (passed_gate) {

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;

perception/tracking_object_merger/src/utils/utils.cpp

@@ -272,11 +272,11 @@
   const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x;
   const auto sub_vy = sub_obj.kinematics.twist_with_covariance.twist.linear.y;
   // calc velocity direction
   const auto main_vyaw = std::atan2(main_vy, main_vx);
   const auto sub_vyaw = std::atan2(sub_vy, sub_vx);
   // get motion yaw angle
   const auto main_motion_yaw = main_yaw + main_vyaw;
   const auto sub_motion_yaw = sub_yaw + sub_vyaw;
   // diff of motion yaw angle
   const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw);
   const auto normalized_motion_yaw_diff =
@@ -324,43 +324,61 @@
   // copy main object at first
   output_kinematics = main_obj.kinematics;
   auto sub_obj_ = sub_obj;
-  // do not merge reverse direction objects
+  // do not merge if motion direction is different
   if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) {
     return output_kinematics;
-  } else if (objectsYawIsReverted(main_obj, sub_obj)) {
-    // revert velocity (revert pose is not necessary because it is not used in tracking)
-    sub_obj_.kinematics.twist_with_covariance.twist.linear.x *= -1.0;
-    sub_obj_.kinematics.twist_with_covariance.twist.linear.y *= -1.0;
   }
 
   // currently only merge vx
   if (policy == MergePolicy::SKIP) {
     return output_kinematics;
   } else if (policy == MergePolicy::OVERWRITE) {
-    output_kinematics.twist_with_covariance.twist.linear.x =
-      sub_obj_.kinematics.twist_with_covariance.twist.linear.x;
+    // use main_obj's orientation
+    // take sub_obj's velocity vector and convert into main_obj's frame, but take only x component
+    const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x;
+    const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y;
+    const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation);
+    const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation);
+    const auto sub_vx_in_main_frame =
+      sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw);
+    output_kinematics.twist_with_covariance.twist.linear.x = sub_vx_in_main_frame;
+
     return output_kinematics;
   } else if (policy == MergePolicy::FUSION) {
+    // use main_obj's orientation
+    // take main_obj's velocity vector and convert into sub_obj's frame, but take only x component
     const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x;
     const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x;
+    const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y;
+    const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation);
+    const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation);
+    const auto sub_vel_in_main_frame_x =
+      sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw);
     // inverse weight
     const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0];
     const auto sub_vx_cov = sub_obj_.kinematics.twist_with_covariance.covariance[0];
+    const auto sub_vy_cov = sub_obj_.kinematics.twist_with_covariance.covariance[7];
+    const auto sub_vel_cov_in_main_frame_x =
+      sub_vx_cov * std::cos(sub_yaw - main_yaw) * std::cos(sub_yaw - main_yaw) +
+      sub_vy_cov * std::sin(sub_yaw - main_yaw) * std::sin(sub_yaw - main_yaw);
     double main_vx_weight, sub_vx_weight;
     if (main_vx_cov == 0.0) {
       main_vx_weight = 1.0 * 1e6;
     } else {
       main_vx_weight = 1.0 / main_vx_cov;
     }
-    if (sub_vx_cov == 0.0) {
+    if (sub_vel_cov_in_main_frame_x == 0.0) {
       sub_vx_weight = 1.0 * 1e6;
     } else {
-      sub_vx_weight = 1.0 / sub_vx_cov;
+      sub_vx_weight = 1.0 / sub_vel_cov_in_main_frame_x;
     }
-    // merge with covariance
+
+    // merge velocity with covariance
     output_kinematics.twist_with_covariance.twist.linear.x =
-      (main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight);
+      (main_vx * main_vx_weight + sub_vel_in_main_frame_x * sub_vx_weight) /
+      (main_vx_weight + sub_vx_weight);
     output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight);
+
     return output_kinematics;
   } else {
     std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl;
@@ -453,21 +471,25 @@
 
 void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj)
 {
+  // do not update if motion direction is different
   if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) {
-    // do not update velocity
-    return;
-  } else if (objectsYawIsReverted(main_obj, sub_obj)) {
-    // revert velocity (revert pose is not necessary because it is not used in tracking)
-    const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x;
-    main_obj = sub_obj;
-    main_obj.kinematics.twist_with_covariance.twist.linear.x = -vx_temp;
     return;
-  } else {
-    // update velocity
-    const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x;
-    main_obj = sub_obj;
-    main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp;
   }
+  // take main_obj's velocity vector and convert into sub_obj's frame
+  // use sub_obj's orientation, but take only x component
+  const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x;
+  const auto main_vy = main_obj.kinematics.twist_with_covariance.twist.linear.y;
+  const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation);
+  const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation);
+  const auto main_vx_in_sub_frame =
+    main_vx * std::cos(main_yaw - sub_yaw) + main_vy * std::sin(main_yaw - sub_yaw);
+
+  // copy sub object to fused object
+  main_obj = sub_obj;
+  // recover main object's velocity
+  main_obj.kinematics.twist_with_covariance.twist.linear.x = main_vx_in_sub_frame;
+
+  return;
 }
 
 void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj)