Merge pull request #1154 from tier4/cherry-pick/taekjin

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) {