diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index 3e2397e427..7b1db618d1 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -9,7 +9,7 @@ autoware_launch/config/control/** takayuki.murooka@tier4.jp fumiya.watanabe@tier
 autoware_launch/config/localization/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp ryu.yamamoto@tier4.jp kento.yabuuchi.2@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp anh.nguyen.2@tier4.jp
 autoware_launch/config/map/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp ryu.yamamoto@tier4.jp kento.yabuuchi.2@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp anh.nguyen.2@tier4.jp
 autoware_launch/config/perception/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp koji.minoda@tier4.jp
-autoware_launch/config/planning/** takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp daniel.sanchez@tier4.jp kyoichi.sugahara@tier4.jp zulfaqar.azmi@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp alqudah.mohammad@tier4.jp maxime.clement@tier4.jp mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp
+autoware_launch/config/planning/** takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp daniel.sanchez@tier4.jp kyoichi.sugahara@tier4.jp zulfaqar.azmi@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp alqudah.mohammad@tier4.jp maxime.clement@tier4.jp mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp yuki.takagi@tier4.jp
 autoware_launch/config/simulator/** takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
 autoware_launch/config/system/** fumihito.ito@tier4.jp isamu.takagi@tier4.jp
 autoware_launch/config/vehicle/** takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp mamoru.sobue@tier4.jp
diff --git a/.github/CODEOWNERS-manual b/.github/CODEOWNERS-manual
index 382818e897..a27c2cc109 100644
--- a/.github/CODEOWNERS-manual
+++ b/.github/CODEOWNERS-manual
@@ -4,7 +4,7 @@ autoware_launch/** yukihiro.saito@tier4.jp ryohsuke.mitsudome@tier4.jp mfc@leodr
 autoware_launch/config/control/** takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
 autoware_launch/config/localization/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp ryu.yamamoto@tier4.jp kento.yabuuchi.2@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp anh.nguyen.2@tier4.jp
 autoware_launch/config/map/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp ryu.yamamoto@tier4.jp kento.yabuuchi.2@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp anh.nguyen.2@tier4.jp
-autoware_launch/config/perception/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp koji.minoda@tier4.jp
+autoware_launch/config/perception/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp koji.minoda@tier4.jp taekjin.lee@tier4.jp
 autoware_launch/config/planning/** takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
 autoware_launch/config/simulator/** takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
 autoware_launch/config/system/** fumihito.ito@tier4.jp isamu.takagi@tier4.jp
@@ -13,7 +13,7 @@ autoware_launch/launch/components/tier4_autoware_api_component.launch.xml isamu.
 autoware_launch/launch/components/tier4_control_component.launch.xml takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
 autoware_launch/launch/components/tier4_localization_component.launch.xml masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp ryu.yamamoto@tier4.jp kento.yabuuchi.2@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp anh.nguyen.2@tier4.jp
 autoware_launch/launch/components/tier4_map_component.launch.xml masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp ryu.yamamoto@tier4.jp kento.yabuuchi.2@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp anh.nguyen.2@tier4.jp
-autoware_launch/launch/components/tier4_perception_component.launch.xml shunsuke.miura@tier4.jp yoshi.ri@tier4.jp koji.minoda@tier4.jp
+autoware_launch/launch/components/tier4_perception_component.launch.xml shunsuke.miura@tier4.jp yoshi.ri@tier4.jp koji.minoda@tier4.jp taekjin.lee@tier4.jp
 autoware_launch/launch/components/tier4_planning_component.launch.xml takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
 autoware_launch/launch/components/tier4_sensing_component.launch.xml shunsuke.miura@tier4.jp yoshi.ri@tier4.jp koji.minoda@tier4.jp
 autoware_launch/launch/components/tier4_simulator_component.launch.xml takayuki.murooka@tier4.jp fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
diff --git a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml
index 135aecc56b..b358e95f86 100644
--- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml
+++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml
@@ -5,6 +5,7 @@
     use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
     admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
     admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
+    use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory
 
     # -- path smoothing --
     enable_path_smoothing: false   # flag for path smoothing
@@ -75,4 +76,4 @@
       average_num: 1000
       steering_offset_limit: 0.02
 
-    debug_publish_predicted_trajectory: true  # publish debug predicted trajectory in Frenet coordinate
+    publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose
diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml
index 3f33218eb3..0237671ccf 100644
--- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml
+++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml
@@ -34,3 +34,6 @@
         low_priority_region_x: -20.0
         center_pcl_shift: 0.0
         radial_divider_angle_deg: 1.0
+
+        # debug parameters
+        publish_processing_time_detail: false
diff --git a/autoware_launch/config/perception/obstacle_segmentation/occupancy_grid_based_outlier_filter/occupancy_grid_map_outlier_filter.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/occupancy_grid_based_outlier_filter/occupancy_grid_map_outlier_filter.param.yaml
index 61cd3a2dc1..20289d02f9 100644
--- a/autoware_launch/config/perception/obstacle_segmentation/occupancy_grid_based_outlier_filter/occupancy_grid_map_outlier_filter.param.yaml
+++ b/autoware_launch/config/perception/obstacle_segmentation/occupancy_grid_based_outlier_filter/occupancy_grid_map_outlier_filter.param.yaml
@@ -10,3 +10,6 @@
     cost_threshold: 45
     use_radius_search_2d_filter: true
     enable_debugger: false
+
+    # debug parameters
+    publish_processing_time_detail: False
diff --git a/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml
index 9dcc722587..eaaeb026d0 100644
--- a/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml
+++ b/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml
@@ -16,3 +16,6 @@
     map_length: 150.0
     map_width: 150.0
     map_resolution: 0.5
+
+    # debug parameters
+    publish_processing_time_detail: false
diff --git a/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml
index 878bea4cd8..8ed3fdddfa 100644
--- a/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml
+++ b/autoware_launch/config/perception/occupancy_grid_map/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml
@@ -12,6 +12,9 @@
         map_length_x: 150.0 # [m]
         map_length_y: 150.0 # [m]
 
+        # debug parameters
+        publish_processing_time_detail: false
+
       # downsample input pointcloud
       downsample_input_pointcloud: true
       downsample_voxel_size: 0.25 # [m]
diff --git a/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml
index a87e8b95a0..c739c68306 100644
--- a/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml
+++ b/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml
@@ -32,3 +32,6 @@
       projection_dz_threshold: 0.01 # [m] for avoiding null division
       obstacle_separation_threshold: 1.0 # [m] fill the interval between obstacles with unknown for this length
       pub_debug_grid: false
+
+    # debug parameters
+    publish_processing_time_detail: false
diff --git a/autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml
index f8a2dc2fbc..42966e01fa 100644
--- a/autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml
+++ b/autoware_launch/config/perception/occupancy_grid_map/synchronized_grid_map_fusion_node.param.yaml
@@ -18,3 +18,6 @@
     fusion_map_length_x: 100.0
     fusion_map_length_y: 100.0
     fusion_map_resolution: 0.5
+
+    # debug parameters
+    publish_processing_time_detail: false
diff --git a/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml b/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml
index 9b7dcffbc6..ecfbdab9e5 100644
--- a/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml
+++ b/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml
@@ -10,3 +10,4 @@
     minimum_reroute_length: 30.0
     consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not.
     check_footprint_inside_lanes: true
+    allow_reroute_in_autonomous_mode: true
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml
index 32d0caffd8..aa1fdb3048 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/dynamic_obstacle_avoidance.param.yaml
@@ -51,6 +51,7 @@
           max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value.
 
       drivable_area_generation:
+        expand_drivable_area: false
         polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base"
         object_path_base:
           min_longitudinal_polygon_margin: 3.0 # [m]
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml
index b1b03c21c1..5eb71126bb 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml
@@ -23,6 +23,10 @@
         ignore_distance_from_lane_start: 0.0
         margin_from_boundary: 0.75
         high_curvature_threshold: 0.1
+        bus_stop_area:
+          use_bus_stop_area: false
+          goal_search_interval: 0.5
+          lateral_offset_interval: 0.25
 
       # occupancy grid map
       occupancy_grid:
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml
index 10eb7486db..a71c202b05 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml
@@ -7,6 +7,7 @@
       th_stopped_time: 1.0
       collision_check_margins: [2.0, 1.0, 0.5, 0.1]
       collision_check_margin_from_front_object: 5.0
+      skip_rear_vehicle_check: false
       extra_width_margin_for_rear_obstacle: 0.5
       th_moving_object_velocity: 1.0
       object_types_to_check_for_path_generation:
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml
index ca1d211d1d..3e99e140c8 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml
@@ -16,7 +16,10 @@
         # For the case where the crosswalk width is very wide
         far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object).
         # For the case where the stop position is determined according to the object position.
-        stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin
+        stop_distance_from_object_preferred: 3.0 # [m]
+        stop_distance_from_object_limit: 3.0 # [m]
+        min_acc_preferred: -1.0 # min acceleration [m/ss]
+        min_jerk_preferred: -1.0 # min jerk [m/sss]
 
       # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false".
       slow_down:
@@ -45,11 +48,10 @@
         ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering
         ego_min_assumed_speed: 2.0 # [m/s] assumed speed to calculate the time to collision point
 
-        no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk
-          max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk.
-          min_acc: -1.0 # min acceleration [m/ss]
-          min_jerk: -1.0 # min jerk [m/sss]
-          max_jerk: 1.0 # max jerk [m/sss]
+        no_stop_decision: # parameters to determine stop cancel. {-$overrun_threshold_length + f($min_acc, $min_jerk)} is compared against distance to stop pose.
+          min_acc: -1.5 # min acceleration [m/ss]
+          min_jerk: -1.5 # min jerk [m/sss]
+          overrun_threshold_length: 1.0 # [m] required to avoid giving up against backward movement of the stop position due to approaching pedestrians, etc.
 
         stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.25 m/s = 0.9 kmph)
         min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph)
@@ -60,6 +62,11 @@
         timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s]
         timeout_ego_stop_for_yield: 1.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough.
 
+      # params for suppressing the ego to restart when the ego is close to the next stop position.
+      restart_suppression:
+        min_distance_to_stop: 0.5
+        max_distance_to_stop: 1.0
+
       # param for target object filtering
       object_filtering:
         crosswalk_attention_range: 2.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk
diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
index c8905f66da..d5d353742c 100644
--- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
+++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
@@ -17,7 +17,7 @@
       ego_cut_line_length: 3.0    # The width of the ego's cut line
       ego_footprint_extra_margin: 0.5 # [m] expand the ego vehicles' footprint by this value on all sides when building the ego footprint path
       keep_obstacle_on_path_time_threshold: 1.0 # [s] How much time a previous run out target obstacle is kept in the run out candidate list if it enters the ego path.
-      keep_stop_point_time: 1.0   # [s] If a stop point is issued by this module, keep the stop point for this many seconds. Only works if approach is disabled
+      keep_stop_point_time: 2.0   # [s] If a stop point is issued by this module, keep the stop point for this many seconds. Only works if approach is disabled
 
       # Parameter to create abstracted dynamic obstacles
       dynamic_obstacle:
@@ -29,7 +29,7 @@
         std_dev_multiplier: 1.96  # [-] min and max velocity of the obstacles are calculated from this value and covariance
         diameter: 0.1             # [m] diameter of obstacles. used for creating dynamic obstacles from points
         height: 2.0               # [m] height of obstacles. used for creating dynamic obstacles from points
-        max_prediction_time: 3.0  # [sec] create predicted path until this time
+        max_prediction_time: 7.0  # [sec] create predicted path until this time
         time_step: 0.5            # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
         points_interval: 0.1      # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
 
@@ -44,7 +44,7 @@
       # Parameter to prevent abrupt stops caused by false positives in perception
       ignore_momentary_detection:
         enable: true
-        time_threshold: 0.5   # [sec] ignores detections that persist for less than this duration
+        time_threshold: 0.15   # [sec] ignores detections that persist for less than this duration
 
       # Typically used when the "detection_method" is set to ObjectWithoutPath or Points
       # Approach if the ego has stopped in front of the obstacle for a certain period
diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml
index 38f8550f72..f5bd2be038 100644
--- a/autoware_launch/launch/autoware.launch.xml
+++ b/autoware_launch/launch/autoware.launch.xml
@@ -131,6 +131,8 @@
       args="-d $(var rviz_config) -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png"
       if="$(var rviz)"
       respawn="$(var rviz_respawn)"
-    />
+    >
+      <env name="QT_QPA_PLATFORMTHEME" value="qt5ct"/>
+    </node>
   </group>
 </launch>
diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml
index 5d7b5eb321..052b4a6429 100644
--- a/autoware_launch/launch/components/tier4_perception_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml
@@ -8,6 +8,7 @@
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_roi_based_cluster" default="true" description="launch roi_based_cluster in clustering"/>
   <arg name="use_low_intensity_cluster_filter" default="true" description="launch low_intensity_cluster_filter in clustering"/>
+  <arg name="use_image_segmentation_based_filter" default="false" description="launch image_segmentation_based_filter in clustering"/>
   <arg name="use_perception_online_evaluator" default="false" description="launch perception online evaluator"/>
   <arg name="use_detection_by_tracker" default="true" description="launch detection_by_tracker function"/>
   <arg name="use_radar_tracking_fusion" default="true" description="if true, radar objects are merged in tracking module. Otherwise, radar objects are merged in detection module."/>
@@ -35,6 +36,7 @@
     <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
     <arg name="use_low_intensity_cluster_filter" value="$(var use_low_intensity_cluster_filter)"/>
+    <arg name="use_image_segmentation_based_filter" value="$(var use_image_segmentation_based_filter)"/>
     <arg name="use_perception_online_evaluator" value="$(var use_perception_online_evaluator)"/>
     <arg name="use_detection_by_tracker" value="$(var use_detection_by_tracker)"/>
     <arg name="use_radar_tracking_fusion" value="$(var use_radar_tracking_fusion)"/>
diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
index 18c391f0db..c50f13e67f 100644
--- a/autoware_launch/rviz/autoware.rviz
+++ b/autoware_launch/rviz/autoware.rviz
@@ -260,6 +260,12 @@ Visualization Manager:
               Turn Signals Topic: /vehicle/status/turn_indicators_status
               Value: true
               Width: 550
+              Background Alpha: 0.5
+              Background Color: 23; 28; 31
+              Dark Traffic Color: 255; 51; 51
+              Handle Angle Scale: 17
+              Light Traffic Color: 255; 153; 153
+              Primary Color: 174; 174; 174
           Enabled: true
           Name: Vehicle
         - Class: rviz_plugins/MrmSummaryOverlayDisplay
@@ -514,26 +520,39 @@ Visualization Manager:
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
                 Value: /localization/pose_estimator/pose_with_covariance
-              Value: true
+              Value: false
             - Buffer Size: 200
-              Class: rviz_plugins::PoseHistory
+              Class: rviz_plugins::PoseWithCovarianceHistory
+              Covariance:
+                Alpha: 0.5
+                Color: 204; 51; 204
+                Scale: 1
+                Value: true
               Enabled: false
-              Line:
-                Color: 170; 255; 127
+              Name: PoseWithCovarianceHistory
+              Path:
+                Shape Type:
+                  Alpha: 0.999
+                  Color: 170; 255; 127
+                  Head Length: 0.20000
+                  Head diameter: 0.30000
+                  Shaft Length: 0.30000
+                  Shaft diameter: 0.15000
+                  Value: Line
+                  Width: 0.10000
                 Value: true
-                Width: 0.10000000149011612
-                Alpha: 0.999
-              Name: PoseHistory
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
-                Value: /localization/pose_estimator/pose
-              Value: true
+                Value: /localization/pose_estimator/pose_with_covariance
+              Value: false
             - Alpha: 0.999
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
@@ -616,20 +635,32 @@ Visualization Manager:
         - Class: rviz_common/Group
           Displays:
             - Buffer Size: 1000
-              Class: rviz_plugins::PoseHistory
+              Class: rviz_plugins::PoseWithCovarianceHistory
+              Covariance:
+                Alpha: 0.5
+                Color: 153; 193; 241
+                Scale: 1
+                Value: false
               Enabled: true
-              Line:
-                Color: 0; 255; 255
+              Name: PoseWithCovarianceHistory
+              Path:
+                Shape Type:
+                  Alpha: 0.999
+                  Color: 0; 255; 255
+                  Head Length: 0.20000
+                  Head diameter: 0.300000
+                  Shaft Length: 0.30000
+                  Shaft diameter: 0.15000
+                  Value: Line
+                  Width: 0.10000
                 Value: true
-                Width: 0.10000000149011612
-                Alpha: 0.999
-              Name: PoseHistory
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
+                Filter size: 10
                 History Policy: Keep Last
                 Reliability Policy: Reliable
-                Value: /localization/pose_twist_fusion_filter/pose
+                Value: /localization/pose_with_covariance
               Value: true
           Enabled: true
           Name: EKF
@@ -937,6 +968,9 @@ Visualization Manager:
               Remaining Distance and Time Topic: /planning/mission_remaining_distance_time
               Enabled: true
               Value: true
+              Background Alpha: 0.5
+              Background Color: 23; 28; 31
+              Text Color: 194; 194; 194
           Enabled: true
           Name: MissionPlanning
         - Class: rviz_common/Group
@@ -2411,6 +2445,36 @@ Visualization Manager:
             Constant Color: false
             Scale: 0.30000001192092896
             Value: false
+        - Class: rviz_plugins/Trajectory
+          Color Border Vel Max: 3
+          Enabled: false
+          Name: Resampled Reference Trajectory
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /control/trajectory_follower/controller_node_exe/debug/resampled_reference_trajectory
+          Value: true
+          View Path:
+            Alpha: 1
+            Color: 153; 193; 241
+            Constant Color: true
+            Value: false
+            Constant Width: true
+            Width: 0.2
+          View Velocity:
+            Alpha: 1
+            Color: 0; 0; 0
+            Constant Color: false
+            Scale: 0.30000001192092896
+            Value: false
+          View Point:
+            Alpha: 1
+            Color: 0; 60; 255
+            Offset: 0
+            Radius: 0.10000000149011612
+            Value: true
         - Class: rviz_default_plugins/Marker
           Enabled: false
           Name: Stop Reason
@@ -3658,7 +3722,7 @@ Visualization Manager:
       Name: Debug
   Enabled: true
   Global Options:
-    Background Color: 42; 42; 42
+    Background Color: 15; 20; 23
     Default Light: true
     Fixed Frame: map
     Frame Rate: 30
@@ -3791,7 +3855,7 @@ Window Geometry:
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ee000005bafc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005500fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb00000024004100750074006f00770061007200650053007400610074006500500061006e0065006c01000000420000041a0000006700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d0061006700650100000505000002680000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000000000000000fb00000030005200650063006f0067006e006900740069006f006e0052006500730075006c0074004f006e0049006d006100670065010000046b000001910000003100fffffffb0000002a004100750074006f0077006100720065004400610074006500540069006d006500500061006e0065006c0000000332000000720000004900fffffffb000000240050006f0069006e00740063006c006f00750064004f006e00430061006d006500720061000000039d000000560000003100ffffff00000001000001ab000005bafc0200000004fb000000100044006900730070006c0061007900730100000042000001f9000000da00fffffffc0000024a000003b2000000d10100001dfa000000000100000002fb0000000a0056006900650077007301000005d5000001ab0000019b00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000009600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7a0000005afc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a0000005afc0100000002fb0000000800540069006d0065010000000000000e7a0000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000746000005ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RecognitionResultOnImage:
     collapsed: false
   Selection: