Merge pull request #760 from tier4/hotfix/fix-object-jerky-yaw

chore(multi_object_tracker): fix jerky object yaw

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -241,6 +241,7 @@
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
+      <arg name="priority_mode" value="0"/>
       <arg name="output/object" value="$(var merger/output/objects)"/>
     </include>
   </group>

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -129,6 +129,7 @@
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="temporary_merged_objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
+      <arg name="priority_mode" value="0"/>
       <arg name="output/object" value="$(var merger/output/objects)"/>
     </include>
   </group>

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

@@ -55,7 +55,7 @@ BicycleTracker::BicycleTracker(
   float q_stddev_y = 0.6;                                     // [m/s]
   float q_stddev_yaw = tier4_autoware_utils::deg2rad(10);     // [rad/s]
   float q_stddev_vx = tier4_autoware_utils::kmph2mps(10);     // [m/(s*s)]
-  float q_stddev_slip = tier4_autoware_utils::deg2rad(25);    // [rad/(s*s)]
+  float q_stddev_slip = tier4_autoware_utils::deg2rad(15);    // [rad/(s*s)]
   float r_stddev_x = 0.6;                                     // [m]
   float r_stddev_y = 0.4;                                     // [m]
   float r_stddev_yaw = tier4_autoware_utils::deg2rad(30);     // [rad]
@@ -135,7 +135,7 @@ BicycleTracker::BicycleTracker(
   ekf_.init(X, P);
 
   // Set lf, lr
-  double point_ratio = 0.5;  // comes to front if larger
+  double point_ratio = 0.2;  // under steered if smaller than 0.5
   lf_ = bounding_box_.length * point_ratio;
   lr_ = bounding_box_.length * (1.0 - point_ratio);
 }

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

@@ -54,7 +54,7 @@ BigVehicleTracker::BigVehicleTracker(
   float q_stddev_y = 1.5;                                     // [m/s]
   float q_stddev_yaw = tier4_autoware_utils::deg2rad(20);     // [rad/s]
   float q_stddev_vx = tier4_autoware_utils::kmph2mps(10);     // [m/(s*s)]
-  float q_stddev_slip = tier4_autoware_utils::deg2rad(20);    // [rad/(s*s)]
+  float q_stddev_slip = tier4_autoware_utils::deg2rad(5);     // [rad/(s*s)]
   float r_stddev_x = 1.5;                                     // [m]
   float r_stddev_y = 0.5;                                     // [m]
   float r_stddev_yaw = tier4_autoware_utils::deg2rad(30);     // [rad]
@@ -150,7 +150,7 @@ BigVehicleTracker::BigVehicleTracker(
   setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 
   // Set lf, lr
-  double point_ratio = 0.5;  // comes to front if larger
+  double point_ratio = 0.2;  // under steered if smaller than 0.5
   lf_ = bounding_box_.length * point_ratio;
   lr_ = bounding_box_.length * (1.0 - point_ratio);
 }

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

@@ -54,7 +54,7 @@ NormalVehicleTracker::NormalVehicleTracker(
   float q_stddev_y = 1.0;                                     // object coordinate [m/s]
   float q_stddev_yaw = tier4_autoware_utils::deg2rad(20);     // map coordinate[rad/s]
   float q_stddev_vx = tier4_autoware_utils::kmph2mps(10);     // object coordinate [m/(s*s)]
-  float q_stddev_slip = tier4_autoware_utils::deg2rad(20);    // object coordinate [rad/(s*s)]
+  float q_stddev_slip = tier4_autoware_utils::deg2rad(5);     // object coordinate [rad/(s*s)]
   float r_stddev_x = 1.0;                                     // object coordinate [m]
   float r_stddev_y = 0.3;                                     // object coordinate [m]
   float r_stddev_yaw = tier4_autoware_utils::deg2rad(30);     // map coordinate [rad]
@@ -150,7 +150,7 @@ NormalVehicleTracker::NormalVehicleTracker(
   setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 
   // Set lf, lr
-  double point_ratio = 0.5;  // comes to front if larger
+  double point_ratio = 0.2;  // under steered if smaller than 0.5
   lf_ = bounding_box_.length * point_ratio;
   lr_ = bounding_box_.length * (1.0 - point_ratio);
 }